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ABSTRACT 



The ability of an autonomous vehicle control system to 
plan a safe, collision-free local path from one vehicle 
position to another is one of the most important 
functions. In this thesis, it is shown how a safe 
obstacle-free local path can be planned using optimal 
control theory and optimization techniques. The problem 
is posed as a two point boundary value problem with 
various problem constraints which control the vehicle 
behavior in transversing from one point to another. The 
objective function being minimized is a control 
performance index which includes vehicle energy saving 
parameters. Numerous fixed and moving obstacles in the 
dive plane are introduced and successfully avoided using 
this technique. Three dimensional path planning is also 
successfully demonstrated on a 12 state linear model of an 
underwater vehicle. This technique is shown to be a 
feasible method for local path planning applications. 
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THESIS DISCLAIMER 

The reaider is cautioneii that computer programs 
(ievelopeii in this research may not have been exerciseci for 
all cases of interest. While every effort has been made, 
within the time available, to ensure that the programs are 
free of computational and logic errors, they can not be 
considered validated. Any application of these programs 
without additional verification is at the risk of the 
user. 
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I . INTRODUCTION 



A . GENERAL 

The presently forecast missions of an Autonomous 
Underwater Vehicle (AUV) vary in scope from mine detection 
and avoidance to surveying the bottom of oceans. Further, 
it is expected that many of these missions will be 
conducted within the context of military objectives. 
Admiral William H. Rowden, Commander Naval Sea Systems 
Command stated that, "With the NAVSEA (Naval Sea System 
Command) Integrated Robotics Program about to enter its 
fifth year of existence, it seems appropriate to look back 
and ahead to establish a baseline for the promulgation of 
policy guidelines to facilitate the continuing evolution 
of this important program." [Ref. 1] He goes on to say 
that the time has come to incorporate the value of 
robotics and automation into the Navy's expanding mission. 
Recent articles of Military Robotics [Refs. 2-6] , have 
pointed out the increased availability of robotic 
vehicles. These include Remotely Piloted Aircraft, 
Unmanned Submarines, Teleoperated Combat Vehicles, Cruise 
Missiles and Teleoperated and Autonomous Weapons. 

An extremely important part of the total AUV vehicle 
control logic is its need to plan and execute a safe 
passage in the undersea environment. Local path planning 
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is the function provided by an intelligent system, which 
determines safe, collision-free trajectory of travel 
between two points, a start point and a target point, for a 
specific time lapse. One possible total system block 
diagram that shows how the local path planner could be 
interfaced, is shown in Figure 1.1. Here, the Global 
Planning System would provide the Local Path Planner with a 
series of data sets. Included in the data sets would be 
destination position, destination time, start position, 
start time, obstacles and boundaries. In return, the path 
planner would provide an optimal path based upon the 
limitations of the vehicle dynamics, power plant 
efficiency, obstacle field, and required maneuver time. 

Numerous techniques have been used to achieve collision 
free local paths for various vehicle types and 
manipulators. These include graphical search methods [Refs 
7-11], potential field methods [Refs. 12-16] and optimal 
control theory [Refs. 17, 18]. This thesis is concerned 
with developing a method of autonomous planning using 
optimal control theory. 

B . PREVIOUS WORK 

A basic investigation of local path planning was 
previously conducted using optimal control theory 
[Ref. 19]. In that study, major emphasis was placed on the 
solution of a SISO (Single Input Single Output) problem, a 
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Figure 1,1 Global Planning System 




MIMO (Multiple Input Multiple Output) problem and its 
generalization to a submersible. That work included 
objective function determination, integration method 
studies, linear versus nonlinear solution results, 
computational expense and an obstacle avoidance solution 
with one fixed obstacle. The objective function used for 
optimization was a quadratic performance index of the form: 



The nonlinear hydrodynamic equations of motion for the 
Autonomous Underwater Vehicle being studied were of the 
following form; 



The "best" solution was obtained by minimizing the 
objective function (J) in order to find the best U(t) and 
X(t) values. 

The Automatic Design Synthesis (ADS) Fortran Program 
[Ref. 20] was utilized for problem optimization and the 
Dynamic Simulation Language (DSL) Program [Ref. 21] was 
utilized for objective function calculations and 
integrations of the vehicle dynamic equations. These 




where , 



U = the control vector; and 



X = desired states-actual states (i.e. state error) 



MX + f(X, X) = g(U) 
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software programs were made to be interactive and now 
perform as one software package [Ref. 22]. The coit±>ined 
package is called ADSL and has been incorporated on the IBM 
3033 Mainframe Computer System at the Naval Postgraduate 
School. The basic optimization approach was as follows: 

1. Discretize the control vector into a time-wise 

uniform distribution of control signals (Figure 1.2) 




TIME (Sec) 



Figure 1.2 Discrete vs. Continuous Controls 

2 . Determine the best control sequence via an 

optimization routine based upon the objective 
function and problem constraints. 

In the two-dimensional problem (dive plane only) , the 

vehicle was ordered to achieve an ordered depth of 17.425 
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feet using minimum bow and stern plane deflections. 
Additionally, the vehicle was required to have a minimum 
pitch angle at its final end condition. The control vector 
(U) was the bow and stern plane angles, while the X vector 
was the x and z positions of the vehicle and velocities in 
the X and z directions. 

C. AIM OF THE PRESENT STUDY 

This thesis is concerned with furthering the 
understanding of local path planning using optimal control 
theory. The purpose of this work is to: 

1. Further develop the planning level control logic 
to consider three-dimensional maneuvers, and 

2. Evaluate the performance of this logic. 
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II. METHOD OF APPROACH 



The basic approach was as follows: 

1. Improve the treatment of obstacles, both fixed and 
moving in the two-dimensional problem. 

2. Determine the best set of optimization program 
options based on computational cost, robustness, 
flexibility and solution accuracy in the two-dimensional 
problem. 

3. Select guidance for maneuvering time (FINTIM) and 
determine how it effects problem solution in the two- 
dimensional problem. 

4 . Evaluate two dimensional versus three dimensional 
computational costs and accuracy. 

The basic assumption in this study was that the work 
previously done [Ref. 19] remained valid. Specifically, 
that the integration method selected, the step size, 
objective function, number of design variables and 
optimization program options remained relevant. 
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OBSTACLE AVOIDANCE 



The approach previously presented [Ref. 19] was to 
compute the distance to the obstacle at ten equally divided 
time intervals from start time to the time of closest 
obstacle approach. These updated distances were then 
incorporated into the optimization algorithm for constraint 
value determination. ADSL placed constraint equations into 
the algorithm in the form: 

Gj (k) =0 j = 1, m 
which in the actual program is: 

Gk(k) = (avoidance zone) - (updated vehicle distance) 
The time interval for distance calculations was determined 
based on the FINTIM and clock time as follows: 

If ( time. ge. 0.0. and. le.xobs/u) then 
timel = xobs/u 

qn = time/ (timel/10. - delt/10000.) 
d = int(qn + 1) 

dist(d) = sqrt( (xpos-xobs) x (zpos-zobs)) 
g(d) = 1. - dist(d) 
where : 

time = DSL clock time 
xobs = X position of the obstacle 
delt = integration time step interval 
xpos = X position of the vehicle 
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u = vehicle velocity in the x direction 

dist(d) = computed distance from the vehicle to the 
obstacle 

zpos = z position of the vehicle 

zobs = z position of the obstacle 

g(d) = constraint value placed in optimization routine 

The problem with this approach is that the further an 
obstacle is from the start position, the longer the time 
intervals become for obstacle distance calculations. This 
is satisfactory for a single fixed obstacle but for 
multiple obstacles, this method results in inadequate 
distance computations. This is because the closer 
obstacles do not have sufficient constraint inputs compared 
to the distant obstacles. As a result, the distant 
obstacles tend to dominate the solution. Using multiple 
"if" statements in this logic is also computationally 
expensive and failed when used with three or more 
obstacles . 

Another inherent problem was that distances were not 
computed after the time of closest approach. This 
sometimes resulted in maneuvers with distances to the 
obstacle that violated the avoidance zone regions after the 
first time of closest approach. 

A better method is to continuously compute distances to 
the obstacle, independently of FINTIM, but dependent on 
FINTIM step intervals. This approach worked very well and 
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was adopted for all further analysis. An additional 
advantage to this approach is that only one constraint 
assignment was needed for each obstacle vice ten. 

The computational time for obstacle avoidance varies as 
the number of obstacles increases. The motivation for this 
study was to determine if this approach was computationally 
too expensive to remain as a viable approach. Figure 3.1 
shows the computational cost for one to seventeen fixed 
obstacles. The computational time is based on the virtual 
machine time for the IBM 3033 system at the Naval 
Postgraduate School. In all cases, the final depth was the 
desired depth of 17.425 feet. 

Various optimization techniques have various 
computational costs; however, the times in Figure 3.1 are 
based upon the final optimization option selected for this 
thesis. The selection criteria with results will be 
presented in the following chapter. 
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IV. OPTIMIZATION OPTIONS 



The ADS (Advanced Design Synthesis) Program allows for 
the selection of numerous optimization techniques for 
problem solution. There are three levels by which to 
select a particular technique. The three levels are the 
strategy level, optimizer level and the one-dimensional 
search level. Table 1 lists the various levels and the 
various methods contained in each. Figure 4.1 identifies 
the large number of possible algorithm combinations 
allowed. Vanderplaats provides a detailed discussion of 
the various methods and algorithms in Reference 23. 

A. CLUSTERED FIXED OBSTACLE TEST 

In order to be effective as a path planning algorithm, 
it is necessary for the program option to be robust and 
flexible enough to solve problems involving numerous fixed 
obstacles as well as moving obstacles. Therefore, an 
initial test was conducted where the number of obstacles in 
the vehicle's path were varied from one to seventeen. 
Program option 057 was selected first based upon the 
recommendation of the previous study, which involved one 
fixed obstacle in the vehicle's path. Option 057 appeared 
acceptable until a four obstacle field was encountered. At 
this point, the option failed to reach an adequate 



lOPT 



OPTIMIZER 



1 Fletcher-Reeves 

2 Davidon-Fletcher-Powell (DFP) 

3 Broydon-Fletcher-Goldfarb-Shanno (BFGS) 

4 Method of Feasible Directions 

5 Modified Method of Feasible Directions 



STRATEGY 



ISTRAT lOPT 12345 



None 0 
SUMT, Exterior 1 
SUMT, Linear Extended Interior 2 
SUMT, Quadratic Extenden Interior 3 
SUMT, Cubic Extended Interior 4 
Augmented Lagrange Multiplier Meth. 5 
Sequential Linear Programming 6 
Method of Centers 7 
Sequential Quadratic Programming 8 
Sequential Convex Programming 9 



X X X X X 
X X X 0 0 
X X X 0 0 
X X X 0 0 
X X X 0 0 
X X X 0 0 
0 0 0 X X 
0 0 0 X X 
0 0 0 X X 
0 0 0 X X 



ONE -DIMENS TONAL SEARCH lONED 



Golden Section Method 1 
Golden Section + Polynomial 2 
Polynomial Interpolation (bounded) 3 
Polynomial Extrapolation 4 
Golden Section Method 5 
Golden Section + Polynomial 6 
Polynomial Interpolation (bounded) 7 
Polynomial Extrapolation 8 



X X X 0 0 
X X X 0 0 
X X X 0 0 
X X X 0 0 
0 0 0 X X 
0 0 0 X X 
0 0 0 X X 
0 0 0 X X 



NOTE: An X denotes an allowed combination of algorithms. 



Figure 4.1 Allowable ADS Algorithm Combinations 



TABLE 1. ADS LEVEL OPTIONS 



STRATEGY (ISRRAT) 

0 - None 

1 - SUMT, Exterior Penalty Function 

2 - SUMT, Linear Extended Interior 

3 - SUMT, Quadratic Extended Interior 

4 - Cubic Extended Interior 

5 - Augmented Lagrange Multiplier Method 

6 - Sequential Linear Programming 

7 - Method of Centers 

8 - Sequential Quadratic Programming 

9 - Sequential Convex Programming 
OPTIMIZER (lOPT) 

1 - Fletcher-Reeves 

2 - Davidon-Fletcher-Powell (DFP) 

3 - Broydon-Fletcher-Golfarb-Shanno (BFGS) 

4 - Method of Feasible Directions 

5 - Modified Method of Feasible Directions 
ONE-DIMENSIONAL SEARCH (lONED) 

1 - Golden Section Method 

2 - Golden Section and Polynomial 

3 - Polynomial Interpolation, bounded 

4 - Polynomial Extrapolation 



5 - Golden Section Method 



6 - Golden Section and Polynomial 

7 - Polynomial Interpolation, bounded 

8 - Polynomial Extrapolation 



1 5 



solution. Option 133 was then selected based upon Olson's 
work [Ref. 22]. Option 133 is more robust than option 057 
and it appeared to be very well suited for this problem 
until the ten obstacle field was encountered. This method 
achieved the correct ordered depth; however, it violated 
many of the obstacle avoidance zones. It was apparent, at 
this point, that all program options would have to be 
tested in order to determine the most appropriate 
algorithm. 

A test was conducted to determine if any of the one 
hundred and twelve program options could solve a seventeen 
obstacle problem. The complete test problem required the 
program option to solve a seventeen obstacle field problem 
with FINTIM set at seven seconds and an ordered depth of 
17.425 feet. Each obstacle had a one foot radius avoidance 
zone. The results of that study are presented in Table 2. 



TABLE 2 



17 OBSTACLE TEST RESULT 



OPTION 


COMPUTATIONAL COST 


DEPTH 




( sec) 


(feet) 


311 


87.92 


17.425 


312 


92.56 


17.425 


313 


58.78 


17.425 


314 


73.51 


17.425 


321 


146.23 


17.425 


322 


142.32 


17.425 


323 


115.11 


17.425 


324 


165.12 


17.425 


331 


167.25 


17.425 


332 


136.84 


17.425 


333 


151.90 


17.425 


334 


120.78 


17.425 


411 


76.19 


17.425 


412 


96.00 


17.421 


413 


42.99 


17.425 


414 


64.92 


17.424 


421 


116.61 


17.425 


422 


140.87 


17.425 


423 


82.18 


17.425 


424 


94.39 


17.425 


431 


115.67 


17.425 


432 


143.50 


17.425 


433 


81.64 


17.425 


434 


97.29 


17.425 


512 


70.17 


17.425 


534 


48.87 


17 .449 



Of the one hundred and twelve program options, only twenty 
six achieved a correct solution. The computational time 
varied significantly among the various successful program 
options. In some cases, the exact depth was not achieved; 
however, all results were considered excellent. 

After determining which algorithms could solve the 
seventeen obstacle problem, it was then necessary to verify 
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that cases involving various obstacle combinations from one 
to sixteen were solvable by these methods. It may seem 
intuitively obvious that if an algorithm can achieve a 
solution involving seventeen obstacles that it can solve 
all other cases from one obstacle to sixteen obstacles. 
Contrary to intuition, this is not the case. For example, 
program option 313, which had a relatively small 
computational cost, successfully solved the seventeen 
obstacle problem but failed to achieve the correct depth 
when an eleven obstacle field was encountered. 

In conducting the varying fixed obstacle investigation, 
obstacles were purposefully placed in various positions in 
the field in order to ensure that obstacle position had no 
negative effect upon the problem solution. This was 
significant because program option 057 (method chosen in 
the previous study) , failed when it encountered an obstacle 
field with three fixed obstacles. Two obstacles were 
placed in the vehicle's path and one was placed far from 
the vehicle's path. The obstacle far away from the 
vehicle's path was determined to be the cause of failure 
because the algorithm successfully solved a problem with 
three obstacles when all three were placed near the 
vehicle's path. Using the cases of one to seventeen 
obstacles, the cases were further reduced from 26 to 22. 
Program options 313, 413, 534, and 314 were eliminated. 



B. IMPOSSIBLE FIELD TEST 



After an investigation of the varying obstacle test, 
the reduced list of programs were subjected to an 
impossible problem. Four obstacles were placed in the 
vehicle's path with nine, five, three, and six feet radii. 
They were placed in such a way that the algorithm could not 
achieve the correct solution in the allotted time. It is 
important to point out that a correct solution would have 
been obtainable if the simulation time was increased. The 
motivation for this study was to determine the failure 
modes of various algorithms. It was evident from this 
study that some algorithms, namely those which employed a 
strategy of Sequential Unconstrained Minimization using the 
Cubic Extended Interior Penalty Function Method, were more 
sensitive to achieving the desired depth constraints, when 
they were imposed as equality constraints. The algorithms 
which employed the strategy of Sequential Unconstrained 
Minimization using the Quadratic Extended Interior Penalty 
Function Method, were more sensitive in avoiding obstacle 
avoidance zones when they were imposed as inequality 
constraints. Figure 4.2 illustrates the performance of 
three different algorithms in solving this problem. Of the 
algorithms with relatively small computational costs, 
program option 311 did the best job of avoiding the 
avoidance zones. 







(li) Hid3a 
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Figure 4.2 Impossible Test Algorithm Comparison 



with the impossible field test, computational cost 
uniformly increased compared to the four obstacle problem 
with smaller avoidance zones. Program option 311 had a 
computational cost of 74.30 Virtual Machine second in the 
impossible problem; however, with four obstacles it took 21 
seconds (Figure 3.1). Figure 4.3 is the solution result 
obtained if FINTIM is increased to 15.0. Although FINTIM 
was more than doubled, the computational cost did not 
significantly increase. With FINTIM set to 15.0, the 
virtual machine time was 76.51 seconds. 

C. SELECTION RESULT 

Program option 311 with a strategy of Sequential 
Unconstrained Minimization using the Quadratic Extended 
Interior Penalty Function Method; an Optimizer using the 
Fletcher-Reeves algorithm and a one-dimensional search 
method using the Golden Section Method was chosen as the 
best algorithm. It was selected because it had the least 
computational cost of any algorithm which could solve the 
seventeen obstacle test problem and was very sensitive to 
the obstacle avoidance zones. In other words, it proved to 
be very good at finding a safe, collision-free path between 
the start condition and the end condition. 
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Figure 4.3 Solution of Impossible Test with Increased FINTIM 



V. EVALUATION OF MANEUVERING TIME (FINTIM) 



Qualitatively, there are two possible FINTIM effects. 
Those which are associated with a small FINTIM and those 
which are associated with an excessively large FINTIM. The 
net effect of too small a FINTIM is an over constraining of 
the problem, which leads to a violation of problem 
constraints and excessive computational time. 

Two things happen when the FINTIM is too large. The 
solution adheres more to problem constraints and the 
computational cost decreases. The mission objectives of 
the vehicle (i.e., loitering at start position), are 
significant considerations, which FINTIM selection must 
take into account. Therefore, FINTIM is a critical 
parameter which effects problem solution and also, when 
chosen correctly, significantly reduces computer 
computational costs. The selection of FINTIM poses an 
important problem which requires solving in view of high- 
level vehicle objectives. 

One guideline for selecting FINTIM is to select it 
based on the time required to achieve a solution while 
transversing an obstacle-free field, then arbitrarily 
increase FINTIM to allow for obstacle avoidance. The 
following results using program options 533 points out the 
importance of choosing a correct FINTIM. As can be seen in 
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Figure 5.1, FINTIM can adversely affect the problem 
solution if the time allotted is not large enough to 
achieve the desired result. When FINTIM is chosen to be 
6.0 non-dimensional time units (NTU) , the avoidance zone 
constraint for zone 3 is violated and the desired depth of 
17.425 feet is exceeded. When FINTIM is increased to 7.0 
NTU, avoidance zone constraints are violated for zone 1 and 
zone 3 and the desired depth is not achieved. However, the 
severity of the violations are not as blatant. When FINTIM 
is increased to 8.0 NTU, the desired problem solution is 
obtained. Table 3 presents the computational costs 
associated with each FINTIM selection. Note that the 
optimization problem is easier with more maneuvering time, 
therefore the computational cost is less. 



TABLE 3. FINTIM COMPUTATIONAL COST 



FINTIM 



TIME (sec) 



6.0 



95.06 



7.0 



76.75 



8 . 0 



32.43 
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FINTIM EFFECTS 
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Figure 5.1 FINTIM Effects 



VI 



PROGRAMMING FOR THREE DIMENSIONS 



In programming for three dimensions, we not only 
optimize the problem to obtain bow plane and stern plane 
commands, but we also optimize to obtain the rudder 
commands. In order to achieve the desired result, it was 
necessary to increase the number of design variables for 
the rudder in the linear model. The problems discussed 
previously, have all been solved using ten discretizations 
(design variables) for the stern plane and bow plane 
inputs. In the three-dimensional work, the number of 
design variables were arbitrarily increased to twenty 
(less discretizations would not achieve a satisfactory 
result) . 

A. SIDE CONSTRAINTS 

Additional constraints were added to the problem in 
order to ensure reasonable vehicle control surface 
reactions. The maximum rudder angles were set at plus or 
minus thirty degrees. In order to ensure this, the side 
constraint approach was invoked. These values were 
assigned to the Design Variable Lower Bound (VLB) and the 
Design Variable Upper Bound (VUB) ADS parameters. 
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B. EQUALITY CONSTRAINTS 

Six additional equality constraints were needed to 
achieve the desired y position and the desired vehicle 
condition (yaw and roll) at the desired end condition. 

C. CONSTRAINT SCALING 

Sanders [Ref. 19] points out that constraint weighting 
is important in achieving the desired results. This is 
even more crucial in a three-dimensional problem solution 
because of the increased number of constraints on yaw, 
roll, rudder control and y positioning. It appears that 
problem sensitivity to constraint weighting is also 
increased. In order to achieve the desired solution 
result, it was necessary to adjust constraint scaling 
factors until all constraint conditions were satisfactorily 
obtained. Table 4 shows the constraint scaling factors 
used in the full three-dimensional linear model. 



TABLE 4. CONSTRAINT SCALING FACTORS 



CONSTRAINT 


SCALING FACTOR 


depth 


0.5 


y position 


2.5 


pitch 


1.0 


yaw 


1.0 


roll 


1.0 


minimum depth 


1.0 
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D. LINEAR/NONLINEAR DYNAMICS 

Figure 6.1 illustrates the nonlinear model behavior 
when using control inputs for bow plane, stern plane, and 
rudder from the optimized linear model. It is evident that 
these commands are invalid for the full scale nonlinear 
model since the final objective state is not closely met. 
Therefore, the essential dynamics of the linear model are 
not valid in three dimensions as might be suggested from 
the results of the previous study. However, even though 
the control surface inputs are invalid, the vehicle state 
trajectory is valid because the path chosen achieved the 
desired result. For a desired position of y=40.0 feet and 
depth =-20.0 feet, the obtained result was y=40.269 feet 
with a depth of -20.699 feet. These values can be fine 
tuned by varying the scaling factors. Figures 6.2 and 6.3 
illustrate the control inputs to the linear model and 
Figure 6.4 illustrates the linear model response to those 
inputs. 
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Figure 6.2 Bow and Stern Plane Control Inputs 
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Figure 6.3 Rudder Control Input 
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Figure 6.4 Linear Model Maneuver 



E. THREE-DIMENSIONAL COMPUTATIONAL COSTS 



Table 5 compares the virtual machine time of the full 
scale linear model with no obstacles as compared to the 
full scale nonlinear model with no obstacles. 



TABLE 5. LINEAR VS. NONLINEAR COMPUTATIONAL COSTS 



LINEAR 

(sec) 

DIVE PLANE ONLY 14.23 

FULL 3D 130.34 



NONLINEAR 

(sec) 

108.81 

370.61 
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VII. VALIDATION RUNS 



As previously mentioned, in order to validate the 
selected optimization configuration, fixed obstacles were 
placed at various positions in the AUVs field of view with 
1.0 foot radius avoidance zones around the obstacles. 
Figure 7.1 to 7.8 illustrate the paths chosen by the 311 
algorithm to avoid the obstacles and their avoidance zones 
for various obstacle positions. 

The moving obstacles were simulated using rectilinear 
average velocity equations of the form: 

s = Vt 

where: 

s = position of obstacle (X and/or Y) 

V = constant velocity 
t = time of travel 

Figures 7.9 to 7.11 present the distance between the AUV 
and the moving obstacle (s) as a function of time. As 
seen, the algorithm chooses a path in both cases which 
avoids impact. Table 6 presents the computational cost 
comparison of various obstacle case(s). Some three- 
dimensional linear and non-linear state trajectory results 
were presented earlier. 
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Figure 7 . l One Obstacle Solution 
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Figure 7.2 Two Obstacles Solution 
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Figure 7.3 Three Obstacles Solution 
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Figure 7.4 Four Obstacles Solution 
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Figure 7.5 Five Obstacles Solution 
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Figure 7.6 Six Obstacles Solution 
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Figure 7.7 Nine Obstacles Solution 
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Figure 7,8 Seventeen Obstacles Solution 
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Figure 7.9 One Moving Obstacle Solution 
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Figure 7.10 Two Moving Obstacles Solution (Obstacle 1) 




45 



Figure 7.10 Two Moving Obstacles Solution (Obstacle 2) 



TABLE 6 



PROGRAM 311 COMPUTATIONAL COST-2 D 



NUMBER OF 


OBSTACLES 


TIME (sec) 


0 




14 .23 


1 


(moving) 


23.88 


2 


(moving) 


24 . 68 


17 


(fixed) 


87.52 
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VIII. CONCLUSIONS AND RECOMMENDATIONS 



A. CONCLUSIONS 

The following conclusions can be drawn from this 
feasibility study: 

1. Optimal control theory is a feasible method for 
determining obstacle avoidance paths in the presence of 
fixed and moving obstacles. 

2. The introduction of obstacle constraints into the 
algorithm increases the computer computational costs. 

3. The full linear model control inputs are not 
compatible with the full nonlinear model. When linear 
commands were placed in the nonlinear model, the vehicle's 
end condition was inconsistent with the desired end 
condition. 

4. The best general algorithm for the two dimensional 
case was determined based on the ability of the algorithm 
to solve a variety of obstacle problems with a shortened 
FINTIM. 

5. Scaling factors can be critical in achieving a 
desired problem solution. 

6. In order to achieve a solution in three 
dimensions, it is necessary to increase the number of 
design variables for certain vehicle control inputs. 
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7. FINTIM is a critical variable whose value can 
change the solution to a specific problem. 



B. RECOMMENDATIONS 

1. Find a procedure to estimate the optimal scaling 
factors for end constraints. 

2. Study the discretization factors in the three- 
dimensional case(s) . 

3. Study the selection of FINTIM in the two-dimensional 
case and in the three-dimensional case. Vehicle 
mission objectives should be considered in 
establishing guidelines. 

4 . Develop the optimization of the three-dimensional 
nonlinear model. 

5. Develop the algorithm to include optimization of 
the propeller rpm control input, 

6. Develop the program for efficient programming in a 
microprocessor. 

7. Determine if the general algorithm recommended in 
this thesis is also applicable for three-dimensional 
obstacle avoidance cases. 
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APPENDIX 



PROGRAMS 



This appendix contains the four primary programs that 
were used for this feasibility study. They were: 

1. OBST DSL - This is the state linear 2D model used 
for the 2D analysis. 

2. TLO DSL - This is the ADSL program used to optimize 
the full scale linear model of bow plane, stern 
plane and rudder control vectors. 

3. TNLO DSL - This is the ADSL program used to optimize 
the full scale nonlinear model for timing comparison 
studies . 

4 . TLNLO DSL - This is the ADSL program used to 
optimize the full scale linear model for bow plane, 
stern plane and rudder control inputs and with 
simulation of the full scale nonlinear model. 



49 



FILE; OBS 



DSL 



A1 



TITLE LINEAR AUV DYNAMIC PATH PLANNER FOR VERTICAL PLANE MOTION 
X SEPARATED BOW AND STERN PLANE CONTROL NON-DIMENSIONAL 

^ 2D STATIONARY OBSTACLES 

ADSL SET UP 

FIXED ISTRAT, lOPT, lONED, IPRINT, INFO, IGRAD, NDV, NCON 
FIXED IDG, NGT, IC, NRA, NCOLA, NRWK, IHK, NRIWK, 0 
D DIMENSION AH(42,A2) 

ARRAY WKC5000), IWKC500) 

ARRAY DX(21), VLB(21), VUB(21), GWC05), DF(21), IDGC05), IC(05) 

PARAM NRA=A2, NC0LA=A2, NRWK=5000, NRIWK=500 
PARAM IGRAD=0, INF0=0, NDV=20, NCON=05, NGT=05 
TABLE DX(l-2)=2^.0, DX( 3-21 ) =1 9^0 . , I DG( 1 -^4 ) = A^-1 

TABLE VLB(l-9)=9^-.17A52, VL B ( 1 1 -1 9 ) =9^- . 2^^3 . VL B( 1 0 ) =0 . ,VLB( 20-21) =0 . 
TABLE VUB(l-9)=9^. 17A52, VUB ( 1 1 -1 9 ) =9^ . 2AA3 , VUB ( 1 0 ) =0 VUB ( 20-21 ) =0 . 
TABLE IDG(5)=01^a 

PARAM ISTRAT=3, IOPT=l, IONED=l, IPRINT=0000 
INCON U=0.0 
METHOD RECT 

CONTROL FINTIM=7.0, DELT=»1 
PRINT XPOS,ZPOS 

^RINT DS,DB, DEPTH, PITCH, XPOS,ZPOS,DT 

X 

XXX DSL MODEL SET 

X 

X 

^ EQUILIBRIUM CONDITION IS CONSTANT SPEED ( NON-DIMENSI ONAL IZED) BY 

^ UO = 6 FT/SEC 

CONST UO=l . 0,A=10,B=11,C=12,D=13,E=1^,F=15,G=16,H=17 
XONST X0BS1=36 . 0 , ZOBSl =-l 2 . 0 , X0BS2=72 . 5 , Z0BS2=-5 . 82 
5(ONST XOBS3 = 9 0 . 0,ZOBS3 = -16 . ,X0BSA = 115. ,Z0BSA = -17 . 
xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 

COrJST X0BS1 = 3A .8 5, ZOBSl = -2.8297 
CONST XOBS2=83.6A,ZOBS2=-12.960 
CONST X0BS3=122 .5,ZOBS3=-2.90 
CONST XOBSA=105.0, Z0BS^=-8.7A 
CONST X0BS5=59 .245, Z0BS5=-7 . 236 3 
CONST XOBS6=35.0, Z0BS6=-14.58 

CONST X0BS7=17.5, Z0BS7=-8.74 
CONST X0BS8=52.5,Z0BS8=-11 .66 
CONST X0BS9=69 .7,ZOBS9=-10 . 155 
CONST XOBS10=70.0, ZOBS10=-2.9 
CONST X0BS11=52.275, ZOBSl 1 =-6 . 1408 
CONST X0BS12=52 .275,Z0BS12=-5 .21 
COrJST X0BS13 = 52.27 5, ZOBSl 3 = -6 . 8836 
CONST X0BS14=50.0, Z0BS14=-5.21 
CONST X0BS15=54.55, Z0BS15=-5.21 
CONST X0BS16=57.5, Z0BS16=-8.74 
CONST XOBS17=35.0, Z0BS17=-8.74 

X 
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uo= 



CONST 



CONST 

CONST 


MA = 


, THETAO= , Z0= ,N0= 


II 

>- 
1— 1 


ZH = 


, ZQ= , ZQDOT= 


, ZMDOT= 


CONST 

X 

CONST 


ZDB = 


, ZDS= , Z0= 




MN = 


, MQ= , MQDOT= 


, MNDOT= 


CONST 


MDB = 


, MDS= , MTHETA= 








INITIAL 

DSAVE1= SQRT((XP0S-X0BS1)X(XP0S-X0BS1)+(ZP0S-Z0BS1)X(ZP0S-Z0B5D) 
DSAVE2=SQRT( (XP0S-X0BS2)^(XP0S-X0BS2)+(ZP0S-Z0BS2)X(ZP0S-Z0BS2) ) 
DSAVE5=SQRT( (XP0S-X0BS3)X(XP0S-X0BS3)+(ZP0S-Z0BS3)^(ZP0S-Z0BS5) ) 
DSAVE^=SQRT( (XPOS-XOBSA)x(XPOS-XOBSA)+(ZPOS-ZOBSA)^(ZPOS-ZOBSA) ) 
DSAVE5=SQRT( (XP0S-X0B55)^(XP0S-X0BS5)+(ZP0S-Z0BS5)^(ZP0S-Z0BS5)) 

DSAVE6=SQRT( (XP0S-X0BS6)X(XP0S-X0BS6)+(ZP0S-Z0BS6)X(ZP0S-Z0BS6) ) 
DSAVE7=SQRT( (XP0S-X0BS7)XCXP0S-X0BS7)+(ZP0S-Z0BS7 )XCZPOS-ZOBS7) ) 
DSAVE8=SQRT( (XP0S-X0BS8)X(XP0S-X0BSS)+(ZP0S-Z0BS8)^(ZP0S-Z0BS8) ) 
D3AVE9=SQRT( (XP0S-X0BS9)^(XP0S-X0BS9)+(ZP0S-Z0BS9)^(ZP0S-Z0BS9) ) 
DSAVEA=SQRT((XPOS-XOBS10)^(XPOS-XOBS10)+. . . 
(ZPOS-ZOBS10)5^(ZPOS-ZOBS10) ) 

DSAVEB=SQRT((XPOS-XOBS10)^(XPOS-XOBS10)+. . . 
(ZPOS-ZOBSIO)X(ZPOS-ZOBSIO) ) 
DSAVEC=SQRT((XPOS-XOBS10)^(XPOS-XOBS10)+. . . 
(ZPOS-ZOBSIO)^(ZPOS-ZOBSIO) ) 

DSAVED=SORT( (XPOS-XOBS10)^(XPOS-XOBS10)+. . . 
(ZPOS-ZOBS10))^(ZPOS-ZOBS10) ) 

DSAVEE=SQRT( (XPOS-XOBS10)X(XPOS-XOBS10)+. , . 
(ZPOS-ZOBSIO)^(ZPOS-ZOBSIO) ) 

DSAVEF=SQRT( (XPOS-XOBS10)^(XPOS-XOBS10)+. . . 
(ZPOS-ZOBSIO)X(ZPOS-ZOBSIO) ) 
DSAVEG=SQRT((XPOS-XOBS10)X(XPOS-XOBS10)+. . . 
(ZPOS-ZOBS10))((2POS“ZOBSIO) ) 
DSAVEH=SQRT((XPOS-XOBS10)x(XPOS-XOBS10)+. . . 
(ZPOS-ZOBSIO)X(ZPOS-ZOBSIO) ) 

ORDDEP = 1.0 
A1=-ZN 

A2=(MA-ZWD0T) 

A3=-(ZQ+MA) 

AA=-ZQDOT 

B1=-MW 

B2=-MND0T 

B3=-MQ 

BA=(IY-MQDOT) 

B5=-MTHETA 

C1=ZDS 

C2=ZDB 

C3=MDS 

CA=MDB 

C5=C3-(BA^C1)/AA 

C6=CA-(BA^C2)/AA 

D1=B2-(BA^A2)/A^ 

D2=B1-(BAXA1)/AA 

D3=B3-(BA^A3)/AA 

CALL DADS(INFO,ISTRAT,IOPT,IONED,IPRINT,IGRAD,NDV,NCON,DX, . . . 

VLB,VUB,OBJ,GN,IDG,NGT,IC,DF,AN,MRA,NCOLA,WK,NRWK, . . . 
IWK,NRINK) 

IF(INFO.EQ.O) THEN 

SAVE THETDD, THETAD, THETA , W, Z, DEPTH, PITCH, DS , DB , BOWANG, STNANG 
ELSE 
ENDIF 

IF(INFO.EQ.O) DELPRT = 0.2 
IF(INFO.EQ.O) DELPLT = 0.2 
DERIVATIVE 
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■[ HETDD=(1/A^)^(C1)(DS+C2XDD-A1XN-A2^^WD0T-A5XTHETAD) 

HDOT= (1/D1)X(C5XDS + C6XDB - D2XN - D3XTHETAD - B5XTHETA) 
THETAD= IUTGRL(THETAO,THETDD) 

THETA = IUTGRL(THETAO,THETAD) 

W = INTGRKWO, WDOT) 

Z = IHTGRL(Z0,N-U0XTHETA) 

DEPTH=-Z 

PITCH=THETA/.017A5 
BONANG=(DB/ .017^5) 

STNAf^G=(DS/. 017^^5) 

IMTGRD = ((HXW+CZ-0RDDEP)X(Z-0RDDEP)+. . . 

THETADXTHETAD+THETAXTHETA) ) + (DSXDS+DBXDB) 

OBJl = INTGRLCO. , (0.5)XINTGRD) 

OBJ = OBJl 

X 

DYNAMIC 

RN=TIME/(FINTIM/10 .-DELT/10000. ) 

0=INT(RN)+1 
IF(O.EQ.ll) 0=10 

X 

X ADDITIONALLY THE PLANES SHOULD BE AT EQUILIBRIUM SO THE 
X VEHICLE MILL PROCEED AT THIS NEW DEPTH WITHIN SOME TOLERANCE 



DS=DX(0) 

DB=DX(10+0) 

IF(O.GE.IO) DS=0. 

IF(O.GE.IO) DB=0. 

X 

X CONSTRAINTS FOR A DIVE 

X 

X ORDERED DEPTH = ORDDEP 
GW(l) =(Z-0RDDEP)/2. 

GW(2) =(0RDDEP“Z)/2. 

X AUV’S FINAL STATE MUST BE LEVEL FLIGHT AS FOLLOWS 
GW(3) = THETA XIO. 

GW(^) = -THETAXIO. 

GW(5)=-ZX100. 

X 

X X-Z POSITIONING FOR OBSTACLE AVOIDANCE 

X 

XP0S=17 . A25XTIME 
ZP0S=-ZX17 . A25 
DT=TIMEX20./FINTIM 



X< 



AVOIDING THE OBSTACLE 

DI3T1=S0RT( (XP0S-X0DSl)K(XP03-X0B51)+( 
DIST2=3QRT( (XP0S-X0BS2)^(XP0S-X0BS2)+( 
DI5T3=SQRT( (XP03-X0BS3)^(XP0S-X0BS3)+( 
DISTA = SORT( (XPOS-XOBSA)5((XPOS-XOB34) + ( 
DIST5=S0RT( (XP05-X0BS5)X(XP0S-X0BS5)+( 
DIST6 = S0RT( (XPOS-XOBS6)5((XPOS-XOBS6) + ( 
DI3T7 = SQRT( (XP03-XOB37 )5((XP03-X0BS7) + ( 
DIST8=SQRT( (XP0S-X0BS8)x(XP0S-X0BS8)+( 
DIST9=SQRT( (XPOS-XOBS9)^(XPOS-XOBS9)+( 
DIST10=SQRT( (XPOS-XOBSIO)X(XPOS-XOBSIO 
(ZPOS-ZOBSIO) ) 

DIST1I=SQRT( (XP0S-X0BS1I)^(XP03-X0BS1I 
(ZPOS-ZOBSID) 

DIST12=S0RT( (XP0S-X0BS12)^(XP0S-X0BS12 
(ZP0S-Z0BS12)) 

DIST13=SQRT( (XP0S-X0BS13)X(XP0S-X0BS13 
(ZP0S-Z0B313)) 

DI3T1A=S0RT( (XP0S-X0BS14)X(XP0S-X0BS1^ 
(ZPOS-ZOBSIA)) 

DIST15=SORT( (XP0S-X0BS15)^(XP0S-X0BS15 
(ZP0S-Z0B315)) 

DIST16=SQRT( (XP0S-X0B316)x(XP0S-X0B316 
(ZPOS-ZOB316)) 

DI3T17 = S0RT( (XP0S-X0B317)5((XP03-X0B317 
(ZPOS-ZOBS17)) 

IFCDISTl .LT.D3AVE1) 



ZP03-Z0B31)5((ZP0S- 

ZP0S-Z0BS2)^(ZP03- 

ZP0S-Z0BS3)X(ZP0S- 

ZPOS-ZOBSA)5((ZPOS- 

ZP03-Z0BS5)^(ZP03- 

ZP0S-Z0BS6)x(ZP0S’ 

ZPOS-ZOBS7)^(ZPOS- 

ZP03-Z0BS8)X(ZP0S- 

ZP0S-Z0BS9)^'(ZP0S- 

)+(ZPOS-ZOBS10)X. 

)+(ZPOS-ZOBSll)X. 

)+(ZP0S-Z0BS12)^. 

)+(ZP0S-Z0BS13)X. 

)+(ZPOS-ZOBSlA)X. 

)+(ZP0S-Z0BS15)X. 

)+(ZP0S-Z0BS16)^. 

)+(ZP0S-Z0B317)X. 



-Z0B3D) 
•Z0BS2)) 
-Z0BS3) ) 
-ZOBSA)) 
-Z0BS5)) 
-ZOBS6) ) 
-ZOBS7)) 
-Z0B38) ) 
-ZOBS9) ) 



IF(DIST2. 
IF(DIST3. 
IFCDISTA. 
IF(DIST5. 
IF(DIST6 . 
IFCDIST7 . 
IFCDIST8 . 
IF(DIST9 . 
IFCDISTIO 
IFCDISTll 
IF(DIST12 
IF(DIST13 
IFCDISTIA 
IF(DIST15 
IF(DIST16 
IF(DIST17 
GN(6)=(9 . 
GN(7)=(5. 
GN(8)=(3. 
GN(9)=(6 , 
GN(10)=(1 



LT.DSAVE2) 
LT.DSAVE3) 
LT.DSAVEA) 
LT.DSAVE5) 
DSAVE6) 
DSAVE7) 
DSAVE8) 
DSAVE9) 
D3AVEA) 
DSAVEB) 
DSAVEC) 
DSAVED) 
DSAVEE) 
DSAVEF) 
DSAVEG) 
DSAVEH) 
-DSAVED 
-DSAVE2) 
-DSAVE3) 
-DSAVEA) 
.-DSAVE5) 



LT. 

LT. 

LT. 

LT. 

.LT. 

.LT. 

.LT. 

. LT, 

. LT. 

.LT. 

.LT, 

.LT. 



DSAVEDDISTl 

DSAVE2=DIST2 

DSAVE5=DIST3 

DSAVEA=DISTA 

D3AVE5=DIST5 

DSAVE6=DIST6 

DSAVE7=DIST7 

DSAVE8=DIST8 

DSAVE9=DIST9 

DSAVEA=DIST10 

DSAVEB=DIST11 

DSAVEC=DIST12 

DSAVED=DIST13 

DSAVEE=DIST1A 

DSAVEF=DIST15 

DSAVEG=DI3T16 

DSAVEH=DIST17 



GW(11)=(1 .-DSAVE6) 
GW(I2)=(1 .-DSAVE7) 
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FILE: OBS 



DSL 



A1 



GM(13)=(1.-D5AVES) 

GW(1^4) = (1 .'DSAVE9) 
GH(15)=(1 .-DSAVEA) 
GW(16)=(1 .-DSAVEB) 
GN(17)=(1 .-DSAVEC) 
GW(1S)=(1.-DSAVED) 
GU(19)=(1 .-DSAVEE) 
GN(20)=(1 .-DSAVEF) 
GN(21)=( 1 .-DSAVEG) 
GN(22)=(1 .-DSAVEH) 

TERMINAL 

IF(INFO.EQ.O) THEN 
PRINTS, D5AVEUD5AVE2 
PRINTS, D5AVE5,D5AVE9 
PRINTS, DSAVE5, DSAVE6 
PRINTS, D5AVE7,D5AVES 
PRINTS, DSAVE9, DSAVEA 
PRir^Tx, DSAVEB, DSAVEC 
PRINTS, D5AVED, DSAVEE 
PRINTS, DSAVEF, DSAVEG 
PRINTS, DSAVEH 
ELSE 
ENDIF 

IF(INFO.EQ.O) CALL ENDJOB 
CALL RERUN 

END 

STOP 
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FILE: TLO DSL A1 



TITLE RUN; 16-5 LINEAR AUV MODEL / STERN PLANE AND BOW PLANE SEPARATED 
^ (1) UPDATED:0A/16/88 
^ (2) 100.00 FT DEPTH CHANGE IN 20 SEC 
^ (3) RIGHT OBJ EQUATION 

(A) ADS CONSTRAINTS ON DEPTH AND PITCH 
^ (5) OBSTACLE FURTHER DOWN THE TRAJECTORY AND ABOVE IT 
^ (6) CORRECT OBSTACLE AVOIDANCE ROUTINE ADDED 



FIXED ISTRAT, lOPT, lONED, IPRINT, INFO, IGRAD, NDV, NCON 
FIXED IDG, NGT, IC, NRA, NCOLA, NRWK, IWK, NRIWK, 0, H,D,C,P 
D DIMENSION AW(A2,A2) 

ARRAY HK(AOOO), IWK(IOOO) 

ARRAY DX(AO), VLB(AO), VUB(AO), GW(ll), DF(Al), IDG(ll), IC(ll) 

PARAM NRA=A2, NC0LA=A2, NRWK=AOOO, NRIWK=1000 
PARAM IGRAD=0, INFO=0, NDV=AO, NC0N=11, NGT=11 
TABLE DX(l-2)=2^. 0,DX(3-21)=19^0 . , IDG( 1-1 0 ) =1 0^-1 
TABLE DX(22-A0)=19^0 . 

TABLE IDG(7-0)=1^1 

TABLE VLB(l-9)=9^-. 17A52, VLB(11-19)=09^-.2AA3,VLB(10)=0.,VLB(20)=0, 
TABLE VUB(l-9)=9^. 17A52, VUB( 1 1-19 ) = 9^ . 2AA3, VUB( 1 0 ) = 0 . ,VUB(20) = 0 . 
TABLE VL B( 21 -39) =19^- .523627, VUB( 21-39) =19^. 5236 27 ,VUB(A0-A1 ) = 0 . 
TABLE VLB(^0-^1)=0. 

PARAM ISTRAT=3, I0PT=1, I0NED=1, IPRINT=0000 
INCON H=0, OBS1=0 . ,YZONE=0 . 

METHOD RECT 

CONTROL FINTIM=21. , DELT=.10 

^RINT XPOSM,YPOSM, DEPTH, THETAM,PHIM,PSIM,DSM, DBM, DRM 
PRINT XPOSN,YPOSM, DEPTH 

^RINT DSM,DBM,DRM,PITCHM,XPOSM,YPOSM, DEPTH, NDT 
^RINT THETAD,W, DEPTH, PITCH, XPOS, DEPTH , NDX , NDZ , NDT 
^AVE THETA, W,Z, DEPTH, PITCH, DS,DB,BOWANG,STNANG 
^RAPH(DE=TEK618) TIME,DS 
^RAPH(DE=TEK618) TIME, DEPTH 
TIME,WDOT 
TIME,W 
TIME,THETDD 
TIME,THETAD 
TIME, THETA 
TIME, PITCH 
TIME,BOWANG 
TIME,STNANG 



^RAPH(DE=TEK618) 

^RAPH(DE=TEK618) 

^RAPH(DE=TEK618) 

^RAPH(DE=TEK618) 

^RAPH(DE=TEK618) 

>^RAPH(DE = TEK618) 

^RAPH(DE=TEK618) 

^RAPH(DE=TEK618) 

D COMMON/BLOCKl/ 

D C0MM0N/BL0CK2/ 

D C0MM0N/BL0CK3/ 

D COMMON/BLOCKA/ 

D C0MM0N/BL0CK5/ 

FIXED N, IA,IDGT,IER,LAST, J,K,M, JJ,KK,I 
INTEGER 

ARRAY WKAREAC5A), X(12) 

CONST 

CONST XPP = 7.03E-3 
XUD0T=-7 .58E-3 
XQDS= 2.61E-2 
XWW = 1.71E-1 
XDSDS=-1 . 16E-2 
XWDSN=3. A6E-3 



S L MODEL FOR LINEAR SIMULATION 
LINEAR MODEL ONLY 

MMINV(6,6), MM(6,6), AA(12,12), BB(6,6) 
B(6,6),A(12,12), UM0D(6),GKK(6,21) 
F(12), FP(6), UCF(A) 
GA(A),GKA(A),BR(A),HH(A) 

XD0T(12) ,XD0TX(12), XD0TUC6) 



LONGITUDINAL HYDRODYNAMIC COEFFICIENTS 



,XQQ = 
,XWQ = 
,XQDB= 
,XVDR= 



-1 . A7E-2 ,XRR = 
-1.92E-1 ,XVP = 
-2.6E-3 ,XRDR= 
1.73E-3 ,XWDS= 
,XDBDB=-8 . 07E-3 ,XDRDR= 
,XDSDSN=-1 .62E-3 



A . OlE-3 
-3.2AE-3 
-8.18E-A 
A .6E-2 
-1 .OlE-2 



,XPR = 
,XVR = 
,XVV = 
,XWDB= 



7 .6AE-A; 



,XQDSN=1 



89E-2, 

29E-2, 

66E-3, 

96E-3, 
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LATERAL HYDRODYNAMIC COEFFICIENTS 



CONST YPDOT=l .27E-^ , YRDOT = 1 . 24 E-3 ,YPQ = 4.125E-3 ,YQR =-6.51E-3,.. 

YVD0T=-5.55E-2 ,YP = 3.055E-3 ,YR = 2.97E-2 ,YVQ = 2.36E-2,.. 

YNP = 2.35E-1 ,YNR = -1.S8E-2 ,YV = -9.31E-2 ,YVN = 6.84E-2,.. 

YDR = 2.73E-2 ,CDY = 3.5E-1 

X 

X NORMAL HYDRODYNAMIC COEFFICIENTS 






CONST ZQDOT=-6 .81E-3 ,ZPP = 1.27E-4 
ZND0T=-2.43E-1 ,ZQ = -1.35E-1 
ZW = -3.02E-1 ,ZVV = -6.84E-2 
ZQN = -2.88E-3 ,ZNN = -5.07E-3 
X 

ROLL HYDRODYNAMIC COEFFITIENTS 

X 

CONST KPDOT=-l .OlE-3 , KRDOT =- 3 . 37 E-5 
KVDOT=l . 27E-4 , KP = -l.lE-2 

KMP = -1.27E-4 , KNR = 1.39E-2 
KPN = -5.73E-4, KDB = 6.94E-3 

X 

PITCH HYDRODYNAMIC COEFFICIENTS 

X 



CONST MQDOT= -1 .68E-2, 
MNDOT= -6. 8 IE- 3, 
MM = 9.86E-2 
MQN = -1.64E-3 , 

X 

X YAH HYDRODYNAMIC 

X 

CONST NPD0T=-3.37E-5 , 
NVDOT=l .24E-3 , 

NWP = -1.75E-2 , 
NDR = -1.29E-2 

X 



MPP = 5.26E-5 
MQ = -6.86E-2 
MW = -2.51E-2 
MWN = -2.88E-3 

COEFFICIENTS 

NRD0T=-3.4E-3 
NP = -8.405E-4 
NWR = 7.35E-3 



,ZPR = 6.67E-3 ,ZRR 
,ZVP = -4.81E-2 ,ZVR 
,ZDS = -7 .255E-2,ZDB 
,ZDSN= -1 . 015E-2,CDZ 



=-7.35E-3, . . 
= 4.55E-2, . . 
= -2 .58E-2, . . 
= 1.0 



,KPQ = -6.93E-5 ,KQR = 1.68E-2,.. 
,KR = -8.41E-4 ,KVQ=-5.115E-3, . . 
,KV = 3.055E-3 , KVW =-1.87E-l,.. 



,MPR = 5.04E-3 ,MRR =-2.86E-2,.. 
,MVP = 1.18E-3 ,MVR = 1.73E-2,.. 
,MDS = -4.12E-2 ,MDB = 6.94E-3,.. 
,MDSN = -5.76E-3 



,NPQ = -2.11E-2 ,NQR = 2.75E-3,.. 
,NR = -1.64E-2 ,NVQ =-9.99E-3,.. 
,NV = -7.42E-3 ,NVW =-2.67E-2,.. 



X MASS CHARACTERISTICS OF THE FLOODED MARK IX VEHICLE 

X 



CONST 


WEIGHT = 15900 


, BOY = 15900 


,VOL = 248.44 


,XG =-0.1 






YG = 0.0 


, ZG = 0.061 


,XB =-0.1 


,ZB =0.023 


r 




IX = 1760 


, lY = 9450 


,IZ = 10700 


,IXZ = -6.65 


9 




lYZ = - 7.0 


, IXY = - 7.0 


,YB = 0.0 




9 




L = 17.425 


, RHO = 1 . 94 


,G = 32.2 


,NU = 1 .5E-5 


9 




AO = 1.57 


,KPROP = 0.0 


,NPROP = 0.0, 


X1TEST=0.01 


9 




DEGRUD= 10.0 


,DEGSTN= 0.0 








CONST 


XOBS1=36.0 











CONST Z0BS1= -12.0 



5 6 



)♦( t\) 



INPUT INITIAL CONDITIONS HERE IF REQUIRED 

K 

INITIAL 

K 

X DSAVEI = SQRT((XPOSM-XOBSI)x(XPOSM-XOBSI)+. . . 

X (ZPOSM-ZOBSl)X(ZPOSM-ZOBSl)) 

X INITIALIZE ALL MATRICES AND ARRAYS TO ZERO 

NOSORT 

ORDDEP=20 . 

YORD= AO. 

D = 0 
H = H+I 

IF (H.EQ.l) THEN 
N = 6 

DO 2 J = 1,N 
JJ= J+N 
DO 1 K = I,N 
KK= K+N 
KKK= KK + N 
MMINV(J,K) = 0.0 
X(J) = 0.0 
X(JJ) = 0.0 
XDOT(J) = 0.0 
XDOT(JJ) = 0.0 
XDOTX(J) = 0.0 
XDOTX(JJ) = 0.0 
XDOTU(J) = 0.0 
UMOD(J) =0.0 
MM(J,K) = 0.0 
BB(J,K) = 0.0 
B(J,K) = 0.0 

AA(J,K)= 0.0 
AA(JJ,KK)= 0.0 
AA(J,KK)= 0.0 
AA(JJ,K)= 0.0 
A(J,KK)= 0.0 
A(JJ,K) = 0.0 
A(J,K) = 0.0 
A(JJ,KK)= 0.0 
GKK(J,K)= 0.0 
GKK( J,KK)=0 . 0 
GKK(J,KKK)=0.0 
CONTINUE 
CONTINUE 

INPUT THE LINEARIZATION POINT PARAMETERS 

UO =6.0 
VO = 0.0 
WO = 0.0 
PO = 0.0 
00 = 0.0 
RO = 0.0 
PHIO = 0.0 
THETAO =0.0 
PSIO = 0.0 
SUM = 0.0 
JFLAG = 0 
IFLAG = 0 
KFLAG = 0 
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^ >K >K ^ >K >K >K >K >K >K >K >K >^< >^ ^ '>K 



INPUT THE MODEL STATES INITIAL CONDITIONS 

UM = 6.0 
VM = 0.0 
HM = 0.0 
PM = 0.0 
QM = 0.0 
RM = 0.0 
XPOSM =0.0 
YPOSM =0.0 
ZPOSM = 0.0 
PHIM = 0.0 
THETAM =0.0 
PSIM = 0.0 



INPUT THE VEHICLE INITIAL CONDITIONS 



INITIALIZE THE CONTROLS 

DBOY= 0.0 
DR=0.0 
DS= 0.00000 
DSM = 0 . 

DBM=0 . 

DB=0 . 000000 
DRM=0.0 
DRPM=0 . 

RPM = 500.00 
LATYAM =0.0 
NORPIT =0.0 



MASS = WEIGHT/G 

DIVAMP = DEGSTN^O. 017^532925 
RUDAMP = DEGRUD^0.017A532925 

THE LINEAR PROPULSION MODEL 



ETA = 0.012^RPM/U0 
ETA = 1.0 
RE = UO^L/NU 

CDO = .00385 + (1 .296E-17)^(RE - 1,2E7)^^2 

CT = 0 . 00S)^U()(2xETAxABS(ETA)/(A0) 

CTl = O . 008^'L)^x2/( AO) 

EPS = -1 . 0+(SQRT(CT+l . 0)-l . 0)/(SQRT(CTl+l . 0)-l . 0) 
XPROP = CDO^(ETA^ABSCETA) - 1.0) 



DO 15 J = 1,N 
DO 10 K =1,N 
MMINVC J,K)=0.0 
MM(J,K) = 0.0 
0 CONTINUE 

5 CONTINUE 



CALCULATE THE MASS MATRIX 



MM(1,1) = MASS -( (RHO/2)^(L^^3)XXUDOT) 
MM(1,5) = MASS^ZG 
MM(1,6) = -MASSXYG 
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>K >K 



= MASS - ( ( RHO/2 ) ^ ( L ^^3 ) ^YVDOT) 
MM(2,A) = -MASS^ZG -( ( RHO/2 ) )XYPDOT ) 

MM(2,6) = MASS^XG - ( ( RHO/2) ^ ) ^YRDOT ) 

MM(3,3) = MASS “ ( ( RHO/2 ) L ^^3 ) XZNDOT) 
MM(3,^) = MASSXYG 

MM(3,5) = -MASS^XG - ( ( RHO/2 ) ^ ( L ) XZQDOT ) 

MM(^,2) = -MASS^ZG - ( (RH0/2)^( LXX4)XKVD0T) 
MM(4,3) = MASS)(YG 

MM(4,4) = IX - ((RH0/2)X(LKX5)^KPD0T) 
MM(4,5) = -IXY 

MM(4,6) = -IXZ -( (RH0/2)X(LX^(5)XKRD0T) 
MM(5,1) = MASSXZG 

MM(5,3) = “MASSXXG -( ( RHO/2) LKX4 )KMWDOT) 

MM(5,4) = -IXY 

MM(5,5) = lY -((RH0/2)K(LXX5)KMQD0T) 

MM(5,6) = -lYZ 

MM(6,1) = -MASS^YG 

MM(6,2) = MASS^XG - ( ( RHO/ 2 ) ^ ( L ^^4 ) XNVDOT ) 
MM(6,4) = -IXZ - ( (RH0/2)^(LX5(5)^NPD0T) 
MM(6,5) = -lYZ 

MM(6,6) = IZ - ((RH0/2)X(LX^5)^NRD0T) 



LAST = N^N+3XN 
DO 20 M = I, LAST 
WKAREA(M) = 0.0 
20 CONTINUE 
X 

lER = 0 
lA = 6 
IDGT = 4 

CALL LINV2F(MM,N, lA, MMINV, IDGT, WKAREA, lER) 

X 

X CALCULATE THE A MATRIX FOR THE LINEAR MODEL 

X 

X 

X 

A(l,l) = RHO/2 XLxx3x(XQDSXDSxQO+XQDB/2XDBxQO+XRDRxROxDR)+. . . 

RHO/2XLXX2X(XVDRXVOXDR+XNDSXDSXWO+XMDB/2XDBXWO + . . . 
2XU0XCXDSDSXDSXX2 + XDBDB/2xDBxx2 + XDRDRxDRxx2) ) + . 
RH0/2xLxx3xXQDSNxQ0xdSXEPS+RH0/2xLxx2x(XWDSNxM0xDS+ . 

2XXDSDSNXUOXDSXX2)XEPS+RHOXLXX2XUOXXPROP+RHO/2XLXX3X 
XQDB/2XDBXQO+RHO/2XLXX2XXHDB/2XDBXWO+RHOXLXX2XUOX . . . 
XDBDB/2XDBXX2 
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FILE: TLO DSL 



A1 















A(l,2) = 
A(l,3) = 
A(1,A) = 
A(l,5) = 

A(l,6) = 
A(l,ll)= 



MASS^RO + RHO/2^LXX3X(XVPXPO+ XVR5(R0) + RH0/2xL?^^2x ... 
(25(XVVXV0 + XVDRXUOXDR) 

-MASS^QO + RHO/2^L^^35((XWQ)^QO) + RHO/2^L^^2^(2^XNW^WO+ . . . 
XNDS^DS^U0+(XNDB/2XDB + XWDB/2)(DB))^U0 +XWDSN^UOXDS5(EPS ) 
-MASS^YG^QO-MASSXZG)(RO+ RHO/2)^L)^^A5(C 2^XPP^P0 + XPR^R0 ) . . . 
+ RHO/2^LX)(3?((XVPXVO) 

-MASS^NO + 2^MASS^XG^QO -MASSXYGXPO + RHO/2XL X)^A)C2XXQQXQO . . 
+ RHO/2^L)^^3X(XWQXWO + XQDSXDS5(UO + XQDB/2^DBXUO) + RHO/2X . . . 
L^)(3XXQDSNXUO)(DS^EPS + RHO/2^L)^X3XXQDB/25(DB^UO 
MASS^VO+2^MASSXXGXRO“MASS^ZG^PO+RHO/2^L^^AX(2^XRRXRO. . . 
+ XPR^PO) + RHO/2^L^)(3^'(XVR)(VO + XRDR5(UO>(DR) 

-(NEIGHT - BOY)XCOS(THETAO) 



A(2,l) = 

A(2,2) = 
A(2,3) = 
A(2,A) = 

A(2,5) = 

A(2,6) = 

A(2, 10)= 
A(2,ll)= 



“MASS^RO + RHO/2^L^X3X(YP^PO + YR^RO ) + RHO/2)^L^^2X(YV^\/0+ . . . 
2^YDRXU0)(DR) 

RHO/2^L^?(3XYVQXQO + RHO/25(LX^2X(YV^UO+YVMXHO) 

MASS5(P0+ RHO/2^U^^3^(YNP?^PO+YWRXRO ) + RHO/2XLX^2^YVW^VO 
MASS^NO-MASS^XG^OO + 2^MASS>CYG^PO + RHO/2XLX^A^YPOXQO+ . . . 
RHO/2^L^^3^(YPXUO+ YNP^NO) 

-MASSXXG^PO-MASS^ZG)^RO + RHO/2^LXXA)((YPQ^PO+YOR)CRO) +. . . 

RHO/2^L^?^3^YVQ^VO 

-MASSXUO + 2^MASS^YG)^RO-h1ASS^ZG^QO + RHO/2^U(5(AXYQRXQO +. . . 

RHO/2^(L)^X3^(YR5(UO + YNR5(N0) 

(NEIGHT - BOY)^CO$(THETAO)5^COS(PHIO) 

-(NEIGHT - BOY)?^SIfKTHETAO)^SirKPHIO) 



A(3, 1) = MASS5(QO + RHO/2?(LX5(3)(ZQ^QO + RHO/2^LXX25((ZWxNO + 2xUO)^ZDSxDS. 

+ 2XUO^ZDB/2)^DB+(ZWN)^NO + 2XZDSH)^UO^DS)XEPS) + RHO/2XL^)(3^ . . 
ZQNXQO^EPS+ RHO/2XL)(^2X2^UOXZDB/2XDB 
A(3,2) = -MASS5^PO + RHO/2^U(X3X(ZVP)(PO+ZVRXRO) + RHO/2XL)^^2X2XZVV)^VO 
A(3,3) = RHO/2^L^X2^(ZW^UO + ZNN^UO^EPS) 

A(3,A) = -MASS^V0-MASS^XG5CR0 + 2^MASS5(ZG>^P0+ RH0/2^L ( 2^ZPPx . . . 

PO + ZPRXRO) + RHO/2XL?(X3)(ZVPXVO 

A(3,5) = MASSXUO - MASS^YG^RO + 2XMASS)(ZGXQO + RHO/25(L^?C35CZQ?(UO +... 
RHO/2^L^^3XZON)(UO^EPS 

A(3,6) =-MASSXXG^PO-MASSXYGXQO + RHO/2XL^xA)((ZPRXPO + 2xZRR^RO)+. . . 
RHO/2^L5(5(3^ZVR^VO 

A(3,10)= -(HEIGHT - BOY ) ^COS ( THETAO )>^SI N( PHI 0 ) 

A(3,ll)= -(HEIGHT - BOY ) x$IN( THETAO ) ^COS( PHI 0 ) 



A(A,1) = 

A(A,2) = 
A(A,3) = 
A(A,A) = 
A(A,5) = 
A(A,6) = 
A(A,10)= 
A(A,11)= 



MASS^YG^^QO + MASS^ZGXRO + RH0/2^L^xA^( KP^PO + ... 
KR5(RO) + RHO/2^L^^'3^(KVXVO + 2XUOX(KDB/2^DB-KDB/2)^DB))+. . . 
RHO/2^L^^3^UOXKPROP+ RH0/2XL ^XA^KPN^POXEPS 
-MASS^YGXPO + RHO/2XLXXAXKVQXQO + RH0/2XL ?^^2x ( KV^^UO . . . 
+ KVH)(HO) 

-MASS^ZGXPO + RHO/2^LX^A?((KHPXPO + KHRXRO) + ... 
RHO/2^L^X3)^KVH^VO 

-IXY^RO + IXZ5(Q0 - MASS^^YG^VO - MASS^ZGXHO + ... 
RHO/2^LX?^5XKPQXQO + RH0/2^L XXA^( KPXUO + KHPXHO ) 

-IZ^^RO + lYXRO + 2)(IYZ^00 + IXZ^PO + MASS^YG^UO +... 
RHO/2?^LX?^5^(KPQXPO + KQR^RO) + RH0/2^L 
-IZXQO + IY)(QO - 2^IYZ^R0 + MASS^ZG^UO + ... 
RHO/2XL^^5XKQRXQO + RH0/2^L ( KR5(U0 + KWRXW0 ) 
-(YG^HEIGHT-YB)(BOY)XCOS(THETAO)XSIN(PHIO) . . . 
-(ZG^HEIGHT-ZB^BOY)XCOS(THETAO))^COS(PHIO) 
-(YG^HEIGHT-YB)CBOY)^SIN(THETAO))(COS(PHIO) . . . 
+(ZG^HEIGHT-ZB^BOY)^SIN(THETAO)XSIN(PHIO) 



BO 



I 


















AC5 


,1) 


AC5 


,2) 


ACS 


,3) 


ACS 




ACS 


,5) 


ACS 


,6) 


ACS 


,10) 


ACS 


,11) 


AC6 


,1) 


AC6 


,2) 


AC6 


,3) 


AC6 


,< 4 ) 


AC6 


,5) 


AC6 


,6) 


AC6 


,10) 


AC6 


,11) 


AC7 


,1) 


AC7 


,2) 


AC7 


,3) 


AC7 


,10) 


AC7 


,11) 


AC7 


,12) 



AC8, 


1) = 


AC8, 


2) = 


AC8, 


3) = 


AC8, 


10) = 


AC8, 


11) = 


AC8, 


12) = 


AC9, 


1) = 


AC9, 


2) = 


AC9, 


3) = 


AC9, 


10) = 


AC9, 


11) = 



-MASS^XG^QO + RHO/2^L^^A^MQ^QO + RHO/ 2^L ^^5^MN^N0 +... 
RHO/2^L^^3>eUO^(MDS^DS+MDB/2^DB) + RHO/25(L A^^MQN^QO^ . . 
EPS + RHO/2XLXX3^(MNNXWO + 2XMDSNXUO^DS ) 5(EPS+ . . . 
RHO/2^L^^35(UOXMDB/2^DB 

MASS^XG^PO + MASS>^<ZG^RO + RHO/25a^5(A^( MVPXPO + ... 
MVR^^RO) + RHO^L^>^3^MVVXVO 

-MASS^ZG^QO + RHO/2^L^X35(MNXUO + RHO/2xL^^3^MNN^UOxEPS 
-IX^RO + IZXRO - lYZxQO - 2XIXZXPO + MASS^XG^VO + ... 
RHO/2^L^^5^(2^MPP^PO + MPR^RO) + RHO/2XL^^A^MVP^VO 
IXYXRO -lYZ^PO - MASS^XG^UO -MASSXZG^NO + RHO/2^... 
LXXA^MQXUO + RHO/2^L5(XAXMQN^UOXEPS 

-IX^PO + IZXPO + IXY^QO + 2^IXZ^RO + MASS^ZG^VO +... 
RHO/2^L^X5X(MPR^PO+2XMRRXRO )+RHO/2^L^^A^MVR^VO 
(XGxWEIGHT-XBxBOY)^COS(THETAO)xSIN(PHIO) 



(XGxWEIGHT-XB^BOY)xSIN(THETAO)?eCOS(PHIO) - ... 
(ZGxNEIGHT-ZB^BOY)5(COS(THETAO) 

“MASS^XG^RO + RHO/2^L^^A^(NP^PO +NRXRO) + RHO/2^... 
L^x3)^(NV^VO + 2XNDR^UO^DR) + RHOXLX^3^UOXNPROP 
-MASS^YG^RO + RHO/25(L^^AXNVQXQO + RH0/2^L ^^3X( NVXU 0+ . . 
uymvio) 

KASSXXG^PO + MASS5(YG^Q0 + RH0/2XL NWP^PO + NNR^RO )+ . 

RHO/2XLXX3¥NVH^VO 

-lYXQO + IX^^QO + 2^IXY^P0 +IYZ^RO + MASS5(XG^W0+ . . . 
RH0/2 ^Lx^5^NPQxQ0 + RHO/2^L^^AX(NPXUO+NNPXWO) 

-lY^PO + IX^PO - Z^IXY^QO - IXZXRO + MASS^YGxWO+ . . . 
RHO/2^L^^5^(NPQXPO+NQRXRO) + RH0/2^L ^^A^NVQ^VO 
lYZ^PO -IXZ^QO - MASS^XG^UO -MASSxYG5(V0 + . . . 
RHO/2XL^^5XNQR^QO + RHO/2^LX5(A^( NR^^UO +NWRXWO) 
(XG^WEIGHT-XB^BOY)^COS(THETAO)XCOS(PHIO) 
-CXG5 (WEIGHT-XB^BOY)?cSIN(THETAO)5(SIN(PHIO) +. . . 
(YG^NEIGHT-YB^BOY)^COS(THETAO) 

C05(PSIO)^COS(THETAO) 

COS(PSIO)5(SIN(THETAO)^SIN(PHIO) - SIN(PSIO)^COS(PHIO) 
COS(PSIO)^SirKTHETAO)^COS(PHIO) + SI N( PSI 0 ) XSI N( PHI 0 ) 
VO^COS(PSIO)^SIN(THETAO)^COS(PHIO) + VOXSIN(PSIO)^. . . 
SIN(PHIO) - WOXCOS(PSIO)XSIN(THETAO)XSIN(PHIO) + ... 
NOXSIN(PSIO)XCOSCPHIO) 

-UO^COS(PSIO)^SIN(THETAO) + VO^COS(PSIO)^COS(THETAO)x. 
SIN(PHIO) + WO^COS(PSIO)5(COS(THETAO)5(COS(PHIO) 
-UO^SIM(PSIO)^COS(THETAO) - V0^SIN(PSI0)XSIN(THETA0)^. 
SIfKPHIO) - VO^COS(PSIO)XCOSCPHIO) - WOxSINC PSI 0 ) ^ . . . 
SIN(THETA0)^SIN(PHI0) + WOXCOS(PSIO)XSIN(PHIO) 

SIN(PSIO)^COS(THETAO) 

SIN(PSI0)xSIN(THETA 0)5(SIN(PHI0) + COS(PSIO)?^COS(PHIO) 
SIM(PSIO)XSIM(THETAO)^COS(PHIO) - COS( PSI 0 ) ^SIN( PHI 0 ) 
VO^SirUPSIO)5(SIN(THETAO)xCOS(PHIO) - VO^COS ( PSI 0 ) x . . . 
SIN(PHIO) - N0XSIN(PSI0)XSIN(THETA0)X$IN(PHI0) - ... 
NOXCOS(PSIO)XCOS(PHIO) 

-U0XSIN(PSI0)XSIN(THETA0) + VOxSINC PSIO)XCOS(THETAO)X. 
SIN(PHIO) + NOXSIN(PSIO)XCOS(THETAO)XCOS(PHIO) 
UOXCOS(PSIO)XCOS(THETAO) + VOxCOSC PSIO )XSIN( THETAO )x . . 
SIN(PHIO) - VOXSIN(PSIO)XCOSCPHIC) + WOXCOSC PSI 0 ) x . . . 
SIN(THETAO)XCOS(PHIO) + WOXSINC PSI 0 ) xSINC PHI 0 ) 

-SIN(THETAO) 

COS(THETAO)XSIN(PHIO) 

COS(THETAO)XCOS(PHIO) 

VO XCOSC THETAO )XC0S( PHI 0)-NOXCOS( THETAO )XSIN( PHI 0) 
-UOXCOS(THETAO)-VOXSIN(THETAO)XSIN(PHIO) -. . . 

WOXSI N( THETAO) XCOSC PHI 0) 
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¥. 

¥. 

X 

X 

X 



A 


( 


10, 


A) 


A 


( 


10, 


5) 


A 


( 


10, 


6 ) 


A 


( 


10, 


10 


A 


( 


10, 


11 



1 . 0 

SIN(PHIO)J^TAfKTHETAO) 

COS(PHIO)J<TAM(THETAO) 

QOXCOS(PHIO)5^TAN(THETAO) - RO^SINC PHI 0 )xTAN( THETAO ) 
QO^SIN(PHIO)/COS(THETAO)X1 .0/COS(THETAO) + ... 
ROxCOS(PHIO)/COS(THETAO)xl . O/COSC THETA 0 ) 



A(ll,5) = COS(PHIO) 

A(ll,6) = -SIN(PHIO) 

Adi, 10)= -QOXSIN(PHIO) - ROXCOS(PHIO) 



A(12,5) = SIN(PHIO)/COS(THETAO) 

A(12,6) = COS(PHIO)/COS(THETAO) 

A(12,10)= QO^COS(PHIO)/COS(THETAO)-R05(SIN(PHIO)/COS(THETAO) 
A(12,ll)= Q03<SIN(PHI0)/C0S(THETA0 )xTAH(THETA0) + ... 

ROXCOS ( PHI 0 )/COS( THETA O)XTAN(THETAO) 



NRITE(10,200)((A(I, J), J=1,12),I=1,12) 



CALCULATE THE B MATRIX 





















60 

70 

SO 



= RHO/2^L^^3^XRDRXUO^RO + RHO/2^L^^2^(XRDR^UO^VO + UOX^2^^.. . 
2^XDRDR^DR) 

B(l,2) = U03<Q0^XQDB/2 + UO^NO^XNDB/2 + UO ^^2^XDBDB^DB 
B(l,3) = UO^QO^XQDB/2 + UO^NO^XNDB/2 + U0^^2^'XDBDB^DB 
B(1,A) = UO^QO^XQDS + UO^WO^XWDS +U0^^2^2^XDSDS^DS+RHO/2^L^^3^ 
XQDSN^UO^'QO^EPS + RH0/2^L^^2^(( XHDSN^UOxWO + 2XXDSDSNX 
U0X5(2^DS)^EPS 

B(l,5) = RH0/2^L^^2^0 . 12^(CD0^0 . 12^(RPM 
B(l,6) = SINCTHETAO) 

BC2,1) = RHO/2^L^^2^YDR^UO^x2 
B(2,6) = -COS(THETAO)XSIN(PHIO) 



B(3,2) 
B(3,3) 
B(3, A) 
B(3,6) 



U0^^2^ZDB/2^RH0/2^L^K2 

U0XX2^2DB/2^RH0/2^L^^2 

U0^X2X2DS^RH0/2^LXX2 + RH0/2XLXX2^2DSNXU0XX2XEPS 
-COS(THETAO)^COS(PHIO) 



B(A,2) =-RH0/2^L^^3^U0xx 2^KDB/2 
B(A,3) = RH0/2^L^^3^U0^x2^KDB/2 

B(A,6) = -YB^COS(THETAO)XCOS(PHIO) + ZB^COSC THETAO ) ^SIN( PHI 0 ) 



B(5,2) 
BC5,3) 
B(5, A) 
B(5,6) 



RH0/2^LXX3^U0xx2^MDB/2 
RHO/2^L^^3^U0^^2^NDB/2 
RH0/2^L^^3^(U0^)^2^MDS+MDSN^U0^X2XEPS) 
XB^COS(THETAO)KCOSCPHIO) + ZBXSI N ( THETAO ) 



B(6,l) = RH0/2^LX?(3^NDRXU0^X2 

B(6,6) = -XBxCOS(THETAO)^SnKPHIO) - YB^SI N ( THETAO ) 



FORMULATE THE A AND B MATRIX FOR STATE SPACE 



REPRESENTATION 



MULTIPLY MMINV AND DF/DX 

DO SO I = 1,6 
DO 70 J = 1,6 
SUM =0.0 
DO 60 K = 1,6 

SUM = SUM + MMINV(I,K)XA(K, J) 
CONTINUE 
AA(I,J) = SUM 
CONTINUE 
CONTINUE 



B2 



^ MULTIPLY MMINV AND DF/DZ 

DO 50 I = 1,6 

DO ^0 J = 7,12 
SUM =0.0 

DO 30 K = 1,6 

SUM = SUM + MMINV(I,K)5(A(K, J) 

30 CONTINUE 

AA(I,J) = SUM 
^0 CONTINUE 

50 CONTINUE 

X 

DO 5 I = 7,12 
DO 6 J = 1,12 
AA(I,J) = A(I,J) 

6 CONTINUE 

5 CONTINUE 

X 

K 

MRITE( 10,200 )((AA(I, J), J = l,12), 1 = 1, 12) 

200 FORMATC 6E12.^) 

^ MULTIPLY MMINV AND DF/DU 

K 

K 

DO no I = 1,6 
DO 100 J = 1,6 
SUM =0.0 
DO 90 K = 1,6 

SUM = SUM + MMINV(I,K)XB(K, J) 

90 CONTINUE 

BB(I,J) = SUM 
100 CONTINUE 

no CONTINUE 

WRITEC 9,300)((BB(I,J),J=1,6),I=1,6) 

300 F0RMAT(6E12.^) 

X 

X 

DO ^05 I = 1,6 

READ (2,^01)(GKK(I, J), J=l,21) 

^05 NRITE(3,^401 HGKKCI, J), J = 1 ,21) 

^01 FORMATC3E20.10) 

X 

X 

ELSE 
END IF 

^ CALL ERRSET (209,256,-1,1,1) 

X PRINT^,INFO,ISTRAT,IOPT,IONED,IPRINT,IGRAD,NDV,NCON,DX, . . . 

X OBJ,GW,IDG,NGT,IC,DF,AM,NRA,NCOLA,NK,NRWK, . . . 

^ IWK,NRIWK 

CALL DADSC INFO, ISTRAT, lOPT, lONED, IPRINT, IGRAD,NDV,NCON, DX, 
VLB, VUB,OBJ,GW, IDG,NGT, IC, DF, AW, NRA, NCOLA, WK, NRWK 
IWK,NRIWK) 

IF (INFO.EQ. 0) DELPRT=0.2 

DERIVATIVE 

NOSORT 
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>K >K >«< 



^'^(kxxlINEAR model 



X CALCULATE BBXU PART OF XDOT = AAXX + BBXU 
X 

DO 10 J = 1,6 
SUM = 0.0 
DO 15 K = 1,6 

SUM = SUM + BBC J,K)XUMOD(K) 

15 CONTINUE 

XDOTUCJ) = SUM 
10 CONTINUE 
X CALCULATE AAXX 

DO 21 J= 1,12 
SUM =0.0 
DO 25 K = 1,12 

SUM = SUM + AA(J,K)XX(K) 

25 CONTINUE 

XDOTXCJ) = SUM 
21 CONTINUE 

X CALCULATE XDOT = AAXX + BBXU 
DO 51 J = 1,6 

XDOTCJ) = XDOTXCJ) + XDOTUCJ) 

31 CONTINUE 

DO 55 J = 7,12 

XDOTCJ) = XDOTXCJ) 

55 CONTINUE 

X 

UDOTM = XDOTCl) 

VDOTM = XDOTC2) 

MDOTM = XDOTC5) 

PDOTM = XDOTCA) 



QDOTM = 
RDOTM = 
XDOTM = 
YDOTM = 
ZDOTM = 
PHMDOT= 
THETMD= 
PSMDOT= 
WRITEC8, 
INTEGRATE 



XD0TC5) 
XD0TC6) 
XDOTC7) 
XD0TC8) 
XDOTC9) 
XDOTCIO) 
XDOTCll) 
XD0TC12) 
600 ) 

XDOT TO 



GET THE STATE VECTOR X 



UM =INTGRLC6 . 0, UDOTM) 

VM= INTGRLCO.O, VDOTM) 

MM= INTGRLCO.O, HDOTM) 

PM= INTGRLCO.O, PDOTM) 

QM= INTGRLCO.O, QDOTM) 

RM= INTGRLCO.O, RDOTM) 

XPOSM = INTGRLCO.O, XDOTM) 
YPOSM = INTGRLCO.O, YDOTM) 
ZPOSM = INTGRLCO.O, ZDOTM) 
PHIM = INTGRLCO.O, PHMDOT) 
THETAM = INTGRLCO.O, THETMD) 
PSIM = INTGRLCO.O, PSMDOT) 



6A 









¥ 



¥ 



X(l) 

X(2) 

X(3) 

X(^i) 

X(5) 

X(6) 

X(7) 

X(3) 

X(9) 

X(IO) 

X(ll) 

XC12J 



UM 

VM 

WM 

PM 

QM 

RM 

XPOSM 

YPOSM 

ZPOSM 

PHIM 

THETAM 

PSIM 



ZDEPTH = ZORD - X( 
THMANG = X( 11)3(57. 
UMODCl )=DRM 
UM0D(2)=DBM 
DBPM= UM0DC3) 
UMOD(3)=DBM 
UMOD(A)=DSM 
UMOD(5)=DRPM 
UMODC 6 ) =DBOY 



9) 

3 



PHANG=PKIM/0 . 017A532925 
THMANG=THETAM/0 . 017 A5 32925 
PSMAfJG= PSiM/ 0.017A532925 
PITCHM = THMANG ‘‘^•^^925 

DEPTH=-2P0SM 



¥ 

X3(3(3(J(3(C0NTR0L 

¥ 



'■^'^^^^^*^^**^^^ii¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥ 



^i^^*ii^ii¥¥¥¥¥¥¥¥¥¥¥ 






DBS 

DBP 

DS 

DR 



UM0DC2) 

UM0DC3) 

UMOD(^) 

UMODCl) 



PUT IN STERN AND BOW PLANE STOPS 






I^BS) . GT . 0 . 6 ) THEN 

ENDIF ' 

then 

DBP - 0.6XABS(DBP)/DBP 
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tF(ABS(DS) -GT .0 .6 ) THEN 
= 0 6*AES(D5)/DS 



ENDIF 



IHTGRII • 

OBJl = INTGRL(0.,(0.5)5(INTGRD) 

OBJ = OBJl 



dynamic ^,„c,.pTv,-rTM/io.-DELT/10000. ) 
?lirTl!5l/ < FH'.Ti!3/20 : -DELT/ lOOOO . ) 



0=1NT(RN)+1 
P=IUT(PN)+1 
IF(O.GE.ll) 
IF(P.GE.20) 



0 = 10 
P = 20 



¥. 

¥i 



XFCP.Gt.^u; r-i-u P 

additionally the planes should^be^at^equilib^ tolerance 



SEHICLrHILL PROCEED AT 



3( 

* 



DSM=DX(0) 
DBM=DX(10+0) 
IF(O.GE.IO) D5M=0 
IF(O.GE.IO) DBM=0 
DRM=DX(20+P) 
rPM=DX( 30+0) 



000 

000 



CONSTRAINTS FOR A DIVE 



AUV’S FINAL STATE MUST 



BE 



Cevel flight as follows 



THETAM 
-THETAM 
PHIM 
-PHIM 
PSIM 

S;?; = iYpSlH-VORD)/.‘i 
giSU).(yORD-YROSI«/.<. 
GW(ll) = -ZPOSM 



GI'K 3) 
GW(^) 
GW(5) 
Gi‘K6 ) 
GI'K 7 ) 
GWCS) 



X 

X 

X 

X 

X 



avoiding the obstacle 



“"3’SsHsg?!™ 

0^6) : 

NDX=XP0SM/17.A25 
NDZ=ZP0SM/17 .A25 

NDT=TIMEX6 ./17 . A25 

terminal 



X 

x66 



9000 



format 

1F( INFO . EQ . 0) THEN 

PRINT5«,0,P 

CONTINUE 

ELSE 

IFCINFO.EQ.O) CALL ENDJOB 
rii I RERUN 



I 



K 

END 

STOP 



FILE: TNLO 



DSL 



A1 



TITLE RUN:16-5 NONLINEAR AUV MODEL / STERN PLANE AND BON PLANE SEPARATED 
^ (1) UPDATED:05/20/8£ 

X (2) RIGHT OBJ EQUATION 

(3) ADS CONSTRAINTS ON DEPTH , PITCH , YAN, ROL L AND Y POSITION 
(A) CORRECTED OBSTACLE AVOIDANCE ROUTINE 

FIXED ISTRAT, lOPT, lONED, IPRINT, INFO, IGRAD, NDV, NCON 
FIXED IDG, NGT, IC, NRA, NCOLA, NRNK, INK, NRINK, 0, H,D,C,PP 
D DIMENSION AN(A2,A2) 

ARRAY NK(5000), INK(IOOO) 

ARRAY DX(AO), VLB(AO), VUB(AO), GN(ll), DF(Al), IDG(II), IC(ll) 

PARAM NRA=A2, NC0LA=A2, NRNK=5000, NRINK=1000 
PARAM IGRAD=0, INFO=0, NDV=AO, NC0N=11, NGT=11 
TABLE DX(1-*2)=2^.0,DX(3-A0) = 58X0. , I DG( 1 -1 0 ) = 1 Ox-1 
TABLE IDG(ll)=lxl 

TABLE VLB(l-09) = 09x-. 17A52, VL B ( 1 1 -1 9 ) = 09x- . 2A A3 , VL B ( 20 ) = 0 . , VL B ( 1 0 ) = 0 . 
TABLE VUB( 1-09) = 09X. 17A52, VUB ( 1 1 -1 9 ) = 09x , 2 A A3 , VUB ( 2 0 ) = 0 . , VUB( 1 0 ) = 0 . 

TABLE VL B ( 21-39 ) = 1 9X-. 6 2367, VUB( 21-39 ) = 19x. 6 23627, VUB( A 0-Al) =2X0 . 

TABLE VLB(A0-A1)=2X0 . 

PARAM ISTRAT=3, I0PT=1, I0NED=1, IPRINT=0000 
INCON H=0, OBS1=0. ,YZONE=0. 

METHOD RECT 

CONTROL FINTIM=21 .0,DELT=.10 

PRINT XPOS,YPOS,ZPOS, PITCH, THEANG, DR, DS 

XRINT THETAD,W, DEPTH, PITCH, XPOS, DEPTH, NDX,NDZ,NDT 

XRINT DS,DB, DR, DEPTH, PITCH, XPOS, YPOS,ZPOS,NDT 

XAVE THETA,N,Z, DEPTH, PITCH, DS,DB, BONANG, STNANG 

XRAPH(DE=TEK618) TIME,DS 

XRAPH(DE=TEK618) TIME, DEPTH 

XRAPH(DE=TEK618) TIME,NDOT 

XRAPH(DE=TEK618) TIME,W 

XRAPH(DE=TEK618) TIME,THETDD 

XRAPH(DE=TEK61S) TIME,THETAD 

XRAPH(DE=TEK618) TIME, THETA 

XRAPH(DE=TEK618) TIME, PITCH 

XRAPH(DE=TEK618) TIME, BONANG 

XRAPH(DE=TEK618) TIME, STNANG 

xxxxxxxxxxxxxxxxxxxxxxxxxxxxdsL model SET UPXxxxxxxxxxxxxxxxxxxxxxxxxxx 

X 

D DIMENSION MM( 6 , 6 ) , GA ( A ) , GKA ( A ) , BR ( A ) , HH ( A ) 

D DIMENSION B(6 , 6 ) , BB(6 , 6 ) 

D DIMENSION A(12,12), AA(12,12) 

D COMMON / BLOCKl / F(12), FP(6), MMINV(6,6), UCF(A) 

FIXED N,IA,IDGT, lER, L AST , J , K , M, J J , KK, I 
INTEGER 



ARRAY 

X 


NKAREAC5A), 


XC12) 








X 

CONST 

X 

X 


LONGITUDINAL 


HYDRODYNAMIC 


COEFFICIENTS 






CONST 


XPP = 


,XQQ = 


,XRR = 


,XPR = 






XUDOT= 


,XNQ = 


,XVP = 


,XVR = 


, 




XQDS = 


,XQDB= 


,XRDR= 


,XVV = 


, 




XNN = 


,XVDR= 


,XNDS= 


,XNDB= 


, 




XDSDS= 


,XDBDB= 


,XDRDR= 


,XQDSN= 


> 


X 


XNDSN= 


,XD5DSN= 
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k' 

k 


LATERA 


L HYDRODYNAMIC COEFFICIENTS 


CONST 


YPDOT= 


,YRDOT= 


/ YPQ = 




YVDOT= 


,YP = 


,YR = 




YHP = 


,YNR = 


.YV = 


k 


YDR = 


,CDY = 




k 

k 


NORMAL 


HYDRODYNAMIC COEFFICIENTS 




CONST 


ZQDOT= 


,ZPP = 


/ ZPR = 




ZWDOT= 


,ZQ = 


,ZVP = 




ZN = 


,ZVV = 


,ZDS = 




ZQN = 


,ZWN = 


.ZDSN= 


k 

k 


ROLL HYDRODYNAMIC COEFFITIENTS 




CONST 


KPDOT= 


, KRDOT= 


,KPQ = 




KVDOT= 


, KP = 


,KR = 




KNP = 


, KWR = 


,KV = 




KPN = 


, KDB = 




k 

k 


PITCH 


HYDRODYNAMIC COEFFICIENTS 




CONST 


MQDOT= 


, MPP = 


,MPR = 




MNDOT= 


, MQ = 


,MVP = 




MN = 


, MW = 


.MDS = 




MQN = 


. MWN = 


,MDSN 


k 


YAW HYDRODYNAMIC COEFFICIENTS 




CONST 


NPDOT= 


, NRDOT= 


.NPQ = 




NVDOT= 


. NP = 


,NR = 




NWP = 


, NWR = 


,NV = 



NDR = 



^ MASS CHARACTERISTICS OF THE FLOODED MARK IX 



k 








CONST 


WEIGHT = 


, BOY = 


WOL = 




YG = 


, ZG = 


,XB = 




IX = 


, lY = 


,IZ = 




lYZ = 


, IXY = 


,YB = 




L = 


, RHO = 


f G = 




AO = 


,KPROP = 


,NPROP 




DEGRUD= 0.0 


,DEGSTN= 0.0 




k 








k 








kONST 


X0BS1=36 .0 






kONST 


ZOBS1=-12.0 






k 









^ INPUT INITIAL CONDITIONS HERE IF REQUIRED 



,YQR = 
,YVQ = 
.YVW = 



,ZRR = 
,ZVR = 
,ZDB = 
,CDZ = 



,KQR = 
,KVQ = 
,KVN = 



,MRR = 
,MVR = 
,MDB = 



,NQR = 
,NVQ = 
,NVN = 



VEHICLE 

,XG = 
,ZB = 
,IXZ = 

,NU = 

, XITEST 



>♦<>*<>*< ^ >K 



INITIAL 

DSAVE1=SQRT( (XPOS-XOBSI)X(XPOS-XOBSI )+(ZP0S-Z0BSl)x(ZP0S-Z0B51 ) ) 

NOSORT 

ORDDEP = 17 .425 
X Y0RD=40.0 



D = 0 
H = H+1 

IF(H.EQ.l) 



THEN 



U 

V 

w 

p 

Q 

R 



0 . 0 
0.0 
0 . 0 
0 . 0 
0.0 
0 . 0 
XPOS =0.0 
YPOS =0.0 
ZPOS =0.0 
PSI = 0.0 
THETA = 0.( 
PHI = 0.0 



6 . 0 
0 . 0 
0.0 
0.0 



UO 
VO 
WO 
PO 

QO = 0 
RO = 0 
PHIO = 
THETAO = 
PSIO = 0. 
DB= 0.0 
DS = 0.0 
DR = 0.0 
RPM = 500 



0 

0 

0 . 0 
= 0.0 
0 . 0 



LATYAW =0.0 
NORPIT =0.0 
RE = UOXL/NU 

CDO = .00385 + (1 .296E-17)x(RE - 1.2E7)xx2 

DEFINE LENGTH FRACTIONS FOR GAUSS QUADUTURE TERMS 

G4(l) = 0.069431844 
G4(2) = 0.330009478 
G4(3) = 0.669990521 
G4(4) = 0.930568155 

DEFINE WEIGHT FRACTIONS FOR GAUSS QUADUTURE TERMS 

GK4(1) = 0.1739274225687 
GK4C2) = 0.3260725774312 
GK4C3) = 0.3260725774312 
GK4C4) = 0.1739274225687 

DEFINE THE BREADTH BB AND HEIGHT HH TERMS FOR THE INTEGRATION 



BR(1) = 75.7/12 
BR(2) = 75.7/12 
BR(3) = 75.7/12 
BR(4) = 55.08/12 



>♦< >K 



HH(1) 

HH(2) 

HH(5) 

HH(^) 

NASS = 

DIVAMP 

RUDANP 



= 16.3S/12 
= 31.85/12 
= 31 .85/12 
= 23.76/12 

NEIGHT/G 

= DEGSTNB(0 
= DEGRUD^O 



017A532925 

017A532925 



10 

15 

X 

X 

X 



X 



X 



X 



N = 6 

DO 15 J = 1,N 
DO 10 K = 1,N 
MMINV(J,K) = 0.0 
NN(J,K) =0.0 
CONTINUE 
CONTINUE 



MM(1,1) = NASS -( (RH0/2)X(LXX3))^XUD0T) 
NM(1,5) = NASS)(ZG 
MM(1,6) = -MASSX'YG 



NM(2,2) = MASS - ( ( RHO/2 ) X ( L ^)(3 ) XYVDOT ) 
MM(2,A) = -NASSXZG - ( ( RHO/2 ) ^ ( L 5(X^ ) XYPDOT ) 
MM(2,6) = MASS^XG - ( ( RHO/2 ) x ( LXX^ ) XYRDOT ) 



MM(3,3) = MASS - ( ( RHO/2 ) )( ( L xx3 ) XZWDOT ) 
MM(3,A) = MASS^YG 

MM(3,5) = -MASSXXG -( ( RH0/2)X( )XZQDOT) 



MM(A,2) = -MASSXZG - ( ( RHO/2 ) X( ) XKVDOT ) 
MM(A,3) = MASSXYG 

MM(A,A) = IX - ((RH0/2)X(L^^5)XKPD0T) 
MM(4,5) = -IXY 

MM(A,6) = -IXZ -((RH0/2)x(LXX5)^KRD0T) 



MM(5,1) = MASSXZG 

MM(5,3) = -MASSXXG -( ( RH0/2))^( LXXA)B(MWDOT) 
MN(5,A) = -IXY 

MM(5,5) = lY -((RH0/2)X(LXX5))(MQD0T) 



X 



X 



MM(5,6) = -lYZ 



MM(6,1) 

MMC6,2) 

MM(6,A) 

MM(6,5) 

MM(6,6) 



-MASSXYG 

MASSXXG -( (RH0/2)^(LXXA)xnVD 0T) 
-IXZ - ( (RH0/2)X( LXX5)XNPD0T) 
-lYZ 

IZ - ((RHO/2)X(L5(X5)xNRDOT) 
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oo oo o >♦<>♦< 



LAST = 

DO 20 M = 1,LAST 
NKAREA(M) =0.0 
20 CONTINUE 

lER = 0 
lA = 6 
I DOT = A 

WRITEC 8,A00)((MM(I, J). J = 1,6), I = 1,6) 

CALL LINV2F(MM,N,IA,MMINV, IDGT , WKAREA, I ER) 

WRITEC 8,A00)((MMINV(I, J), J = 1,6), I = 1,6) 
AOO F0RMAT(6E12.<i) 

X 

ELSE 

ENDIF 



CALL DADS(INFO,ISTRAT,IOPT,IONED,IPRINT,IGRAD,NDV,NCON,DX, . . . 

VLB,VUB,OBJ,GW,IDG,NGT, IC, DF, AN,NRA,NCOLA,WK,NRWK, . . 
IWK,NRIWK) 

IF(H.EQ.l) THEN 
NK(12) = .002 

CALL DADS(INFO,ISTRAT,IOPT,IONED,IPRINT,IGRAD,NDV,NCON,DX, . . . 

VLB,VUB, OBJ,GW, IDG,NGT, IC, DF, AW,NRA,NCOLA,WK,NRNK, . . 
IHK,NRIWK) 

IF(INFO.EQ.O) DELPRT = 0.2 
^ IF(INFO.EQ.O) DELPLT = 0.2 

DERIVATIVE 

NOSORT 

^ PROPULSION MODEL 

X 

X 

SIGNU =1.0 

IF (U.LT.0.0) SIGNU = -1.0 
IF (ABS(U) .LT.XITEST) U = XITEST 
SIGNN = 1.0 

IF (RPM.LT.0.0) SIGNN = -1 .0 
ETA = 0.012^RPM/U 
RE = U^L/NU 

CDO = .00585 + (1 .296E-17)5((RE - 1.2E7)^^2 

CT = 0.008XL^^2^ETA^ABS(ETA)/(A0) 

CTl = 0.008^L5(^2/(A0) 

EPS = -1.0+SIGNN/SIGNU^(SQRT(CT+1.0)-1.0)/(SQRT(CT1+1.0)-1.0) 
XPROP = CD05^(ETA^ABS(ETA) - l.O) 

X 

X 

X 

^ CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER THE VEHICLE 

^ INTEGRATE USING A A TERM GAUSS QUADUTURE 

X 

X 

LATYAW =0.0 
NORPIT =0.0 
DO 500 K = 1,A 

UCF(K) = SQRTC (V+GA(K)XRXL)^^2 + ( H-GA( K)5(QXL ) ^^2) 
IFCUCFCK) .GT.lE-10) THEN 
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FILE: TNLO 



DSL 



A1 



500 

X 

X 

X 

X 

X 

X 

X 



TERMO = (RH0/2)X(CDY*HH(K)X(V+GA(K)XRXL)XX2 +... 

CDZXBR(K)X(W-GA(K)J(QXL)XX2) 

TERMl = TERM0X(V+GA(K)XRXL)/UCF(K) 

TERM2 = TERMOJ«(W-GA(K)*QJ(L)/UCF(K) 

LATYAW = LATYAW + TERMl^GKAC K)XL 
NORPIT = NORPIT + TERM2XGKA ( K ) XL 
END IF 
CONTINUE 



FORCE EQUATIONS 



LONGITUDINAL FORCE 

FP(1) = MASSXVXR - MASSs^WXQ + MASSXXG?(Q’(X2 + MASSXXGXRXX2- . . . 

MASS^YG?(PJ«Q - MASS5<ZG)(P)(R + ( RHO/2 ) 5«L ( XPPxpxxZ +... 

XQQXQXX2 + XRR>iR5«>s2 + XPRJ(PXR) + ( RHO/2 ) XL*?(3X( XWQJa-JXQ +... 
XVP^V)(P+XVR)(V)'R+UXQX(XODSxDS+XODB*DB) + XRDR)fUJ(R)(DR)+. . . 
(RH0/2)XLXX2^((XVV^VXX2 + XHNxwxx2 + XVDRXU^(V5<DR + UxWx... 
(XWDSXDS+XWDBxDB)+Uxx2*(XDSDS5<DSXX2+XDBDB*DB5(3<2+ . . . 
XDRDRXDRXX2) )-(HEIGHT -BOY ) XSIN( THETA ) + ( RHO/2 ) XL XX3X ... 
XQDSNxUxQxdSXEPS+(RH0/2)xLxx2x(XWDSNxUxWXDS+XDSDSNxUXX2x. . 
DSXX2)xePS +(RH0/2)xlxx2xuxx2XXPR0P 



LATERAL FORCE 



FP(2) 



= -MASSxuxr - MASSXXGXPXQ + MASSXYGXRXX2 
(RH0/2)XLXXAX(YPQXPXQ + YQRXQXR)+(RH0/2) 
YRXUXR + YVQXVXQ + YHPxwxp + YWRXHXR) + 
(YVXUXV + YVWXVXW +YDRXUXX2XDR) -LATYAH 
COS(THETA)XSIN(PHI) 



- MASSxZGXQxr + . . . 
XLXX3XC YPXUXP + . . . 
CRH0/2)xlxx2x . . . 
+ (WEIGHT-BOY)x. . . 



NORMAL FORCE 



FP(3) = MASSXUXQ - MASSXVXP - MASSxXGXpxR - MASSXYGxQxR + 

MASSXZGXPXX2 + MASSXZGXQXX2 + ( RH0/2)XLXXAX(ZPPXPXX2 +... 
ZPRxpxR + ZRRXRXX2) + ( RHO/2) xLxx3x( ZQXUXQ + ZVPXVXP +... 
ZVRxVxr) +(RH0/2)xlxx2x(ZHxUxW + ZVVXVxx2 + UXX2X(ZDSX. . . 
DS+ZDBxDB) )-NORPIT+ (HEIGHT-BOY )XC0S( THETA )XCOS(PHI)+ . . . 
(RHO/2)xlxx3XZQNXUXQxePS +(RH0/2)xlxx2x(ZWNXUXW +ZDSNX... 
UXX2XDS)XEPS 



ROLL FORCE 



FP(A) = -IZXQXR +IYXQXR -IXYXPXR +IYZXQXX2 -IYZXRXX2 +IXZxpxQ + 
MASSXYGXUXQ -MASSXYGXVXP -MASSXZGXHXP+ ( RHO/2 ) XL xx5x( KPQX 
PXQ + KQRXQxR) +(RH0/2)xlxxAX(KPXUXP +KRXUXR + KVQXVXQ + 
KHPXHXP + KHRXWXR) +( RHO/2) xLxx3x( KVxUXV + KVWXVXW) + . 

(YGXWEIGHT - YBXBOY ) XCOS ( THETA ) XCOS ( PHI) - (ZGXHEIGHT - 
ZBXBOY)XCOS(THETA)xsIN(PHI ) + (RH0/2)XLXXAXKPNXUXPXEPS+ 
(RHO/2) XL XX3XUXX2XKPR0P +MASSXZGXUXR 
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I 



>*: xoK ^ ^ CTv 












00 



X 



X 

X 

X 

X 



PITCH FORCE 
FP(5) 



YAW FORCE 



-IX2«„r . 

U«v ♦ „vn,v»w » NDRK2^o;)'-\T5i./I"?5^^>*^« 
XB5fB0Y))(C0S(THFTAl^^TM,n.r ‘■^^YAW + (XG^WEIGHT - .. 

+ (RHO/2)XLXJ(3XUxx2xnprop-^bxbOyJsTn?THETA)^^^^^^^^ • 

^''^^•|? 40 - 0 )THEN ’ 

fiTE (S,500)(FP(I), I = i,6) 

END IF 

NOW COMPUTE THE F(I-6) FUNCTIONS 
DO 600 J = 2,6 
DO 600 

CONTINUE^'^^ " + p(J) 

ZZ.7JIZZT --” 0 - 

int DRIFT current VALUES 

UCO = 0.0 
VCO = 0.0 
NCO =0.0 



inertial position rates F(7-9) 



^''X'^OSaHETA^SINCPHI, *«»COS( THEIA)« 

EULER ANGLE RATES F(10-12) 

F(IO) = p + OXSlN(PHI)j(TAN(THFTa^ x r, 

= 0,C0S(P„I, - p,si„,P„i, ’'«'^°=<^"I»TAN, THETA, 

= -STH.PHT.eoS.THETA, , PHCOS, PHn.COS, THETA, 
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>K >♦< 



1 > 12 ) 



X 

^ IF (Z. EQ. 1 . 0)WRITE ( 9 . 50 0 ) ( FC I ) , I = 

XOO F0RMATC6E12 .^4) 

^ Z = Z + 1 

X 

UDOT = F(l) 

VDOT = F(2) 

NDOT = F(3) 

PDOT = F(A) 

QDOT = F(5) 

RDOT = F(6) 

XDOT = F(7) 

YDOT = F(5) 

ZDOT = F(9) 

PHIDOT = F(IO) 

THETAD = F(ll) 

PSIDOT = F(12) 

U = INTGRL (UO,UDOT) 

X X(l) = U 

V = INTGRLCO .0, VDOT) 

X X(2) = V 

N = INTGRL(0.0,NDOT) 

X X(3) = N 

P = INTGRL(0.0,PDOT) 

X X(A) = P 

Q = INTGRLCO.O^QDOT) 

X X(5) = Q 

R = INTGRL(0.0,RDOT) 

X X(6) = R 

XPOS = INTGRL(0.0,XDOT) 

^ y f 7 ^ = y PD*^ 

YPOS = INTGRKO.O^YDOT) 

X X(S) = YPOS 

Z = INTGRL(0.0,ZDOT) 

X X(9) = ZPOS 

PHI = INTGRKO.O, PHIDOT) 

X X(10) = PHI 

THETA = INTGRL(0.0,THETAD) 

X XCll) = THETA 

PSI = INTGRLCO.O, PSIDOT) 

XC12) = PSI 

PHIArJG = PHI/0. 017A532925 
THEANG = THETA/0.017A532925 
PSIANG = PSI/0.017A532925 
ZPOS=-Z 

X 

DEPTH=ZPOS 
PITCH=THEANG 
B0NANG=(DB/.017A5) 

STMANG=(DS/ . 017A5) 

IMTGRD = (UXU+VXV+WXW+PXP+QXQ+RXR+XP0SXXP0S+(YP0S-Y0RD)X. . 
CYPOS-YORD)+CZ-ORDDEP)X(Z-ORDDEP)+PHIxPHI+. . . 
THETAXTHETA+PSIXPSI) + ( DSxDS+DBxdB )+( DRXDR) 

OBJl = lUTGRUO. ,(0.5)XINTGRD) 

OBJ = OBJl 
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X 

DYNAMIC 

RN = TIME/(FINTIM/10 .-DELT/1 00 00 . ) 

PN = TIME/(FINTIM/20 . -DELT/10000 . ) 

0=INT(RN)+1 
PP=INT(PN)+1 
IF(O.GE.IO) 0=10 
IF(PP.GE.20) PP=20 

X 

X ADDITIONALLY THE PLANES SHOULD BE AT EQUILIBRIUM SO THE 
X VEHICLE WILL PROCEED AT THIS NEW DEPTH WITHIN SOME TOLERANCE 
K 

DS=DX(0) 

DB=DX(10+O) 

IFCO.GE.IO) DS=0. 

IF(O.GE.IO) DB=0. 

DR=DX(20+PP) 

X RPM=DX(30+O) 

X 

X CONSTRAINTS FOR A DIVE 

X 

X ORDERED DEPTH = ORDDEP 

GN(1) = (Z-0RDDEP)X.5 
GW(2) = (0RDDEP-Z)x.5 

X AUV»S FINAL STATE MUST BE LEVEL FLIGHT AS FOLLOWS 
GN(3) = THETA 
GW(A) = -THETA 
GN(5)= (YPOS-YORD)/ . A 
GW(6)= (YORD-YPOS)/. A 
GN(7)=PSI 
GW(S)=-PSI 
GN(9)=PHI 
GW(10)=-PHI 
GW(11)=ZP0S 
X 

X AVOIDING THE OBSTACLE 

X DIST1=SQRT( (XP0S-X0BS1)X(XP0S-X0BS1)+(ZP0S-Z0BS1)X(ZP0S-Z0BSD) 
X IF (DISTl .LT.DSAVEl) DSAVE1=DIST1 

X GW(6) = (l.-DSAVEl) 

NDX=XP0S/17 . A25 
NDZ=ZP0S/17 . A25 
NDT=TIMEX6 ./17 . A25 
TERMINAL 

IFCINFO.EQ. 0) THEN 
X PRINTx,DSAVE1 

X9999 F0RMAT(1X,E15.A) 

9000 CONTINUE 
ELSE 
ENDIF 

IFCINFO.EQ. 0) CALL ENDJOB 
CALL RERUN 
K 

END 

STOP 
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FILE: TX 



DSL 



A1 



TITLE LINEAR AUV MODEL / STERN PLANE AND BOW PLANE SEPARATED 
TITLE WITH COMMANDS TO NONLINEAR MODEL 
X (1) UPDATED: 05/30/88 
(3) RIGHT OBJ EQUATION 
X (A) ADS CONSTRAINTS ON DEPTH AND PITCH 
^ (5) OBSTACLE FURTHER DOWN THE TRAJECTORY AND ABOVE IT 
^ (6) CORRECT OBSTACLE AVOIDANCE ROUTItiE ADDED 

X XXX ADSL SET-UP^5(xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 
FIXED ISTRAT, lOPT, lONED, IPRINT, INFO, IGRAD, NDV, NCON 
FIXED IDG, NGT, IC, NRA, NCOLA, NRWK, IWK, NRIWK, 0, H,D,C,PP 
D DIMENSION AW(A2,A2) 

ARRAY WK(AOOO), IWK(IOOO) 

ARRAY DX(AO), VLB(AO), VUB(AO), GWC07), DF(Al), IDGC07), ICC07) 

PARAM NRA=A2, NC0LA=A2, NRWK=A000, NRIWK=1000 
PARAM IGRAD=0, INFO^O, NDV=AO, NC0N=07, NGT=07 
TABLE DX(l-2)=2x.0,DX(3-21)=19^0 . , I DG( 1-6 ) =6^-1 
TABLE DX(22-A0)=19X0. 

TABLE IDGC7-0)=1X1 

TABLE VLB(1-9)=9x-.17A52, VL B( 1 1 -1 9 ) =9^- . 2 AA3 , VL B ( 1 0 ) = 0 . , VL B ( 20 ) =0 . 
TABLE VUB( 1“9)=9^. 17A52, VUB ( 1 1 - 1 9 ) =9 X . 2 A9 3 . VUB ( 1 0 ) = 0 . , VUB ( 20 ) = 0 . 

TABLE VL B( 21-39) =19^". 523627, VUB( 21-39) =19^. 523627, VUB(A0"A1 ) =2^0. 

TABLE VLB( AO-Al )=2X0 . 

PARAM ISTRAT=3, I0PT=1, I0NED=1, IPRINT=0000 
INCON H=0, OBS1=0.,YZONE=0. 

METHOD RECT 

CONTROL FINTIM=21., DELT=.l 

PRINT XP0S,YP05,ZP0S,XP05M,YP0SM,2P0SM 

^RINT D5,DSM,DBPM,DBP,PITCHM,PITCH,XP0SM,YP0SM,XP0S,YP0S,ZP0SM,ZP0S,NDT 
XRINT YPOSM,YPOS 

XRINT THETAD,W, DEPTH, PITCH, XPOS, DEPTH, NDX,NDZ,NDT 
XAVE THETA, W,Z, DEPTH, PITCH, DS, DB, BOWANG, STNANG 
xRAPH(DE=TEK618) TIME,DS 
XRAPH(DE=TEK618) TIME, DEPTH 
TIME,NDOT 
TIME,N 
TIME,THETDD 
TIME,THETAD 
TIME, THETA 
TIME, PITCH 
TIME, BOWANG 
TIME, STNANG 



XRAPH(DE=TEK618) 

XRAPH(DE=TEK618) 

XRAPH(DE=TEK618) 

XRAPH(DE=TEK6I8) 

XRAPH(DE=TEK618) 

XRAPH(DE=TEK618) 

XRAPH(DE=TEK618) 

XRAPH(DE=TEK618) 

xxxxxxxxxxxxxxxxxxxD S L MODEL FOR LINEAR SIMULATION xxxxxxxxxxxxxxxxxxxx 



XXXXXXXXXXXXXXXXXXAND 

X 

D COMMON/BLOCKl/ 

D C0MM0N/BL0CK2/ 

D C0MM0N/BL0CK3/ 

D COMMON/BLOCKA/ 

D C0MM0N/BL0CK5/ 

FIXED N, lA, IDGT, IER,LAST, J,K,M, JJ,KK, I 
INTEGER 

ARRAY WKAREAC5A), X(12) 

X 

X 

X 



NON-LINEAR SIMULATION VARYING CONTROL L AWXXXXXXXXX) 
LINEAR MODEL/NON-LINEAR MODEL 



MMINV(6,6), MM(6,6), AA(12,12) 
B(6,6),A(12,12), UM0DC6) ,GKK(6 
F(12), FP(6), UCF(A) 
GA(A),GKA(A),BR(A),HH(A) 
XD0T(12),XD0TX(12), XD0TUC6) 



BB(6,6) 
21 ) 
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CONST 





LONGITUDINAL 


HYDRODYNAMIC COEFFICIENTS 






CONST 


XPP = 


,XQQ = 


,XRR = 




,XPR = 




XUDOT= 


,XWQ = 


,XVP = 




,XVR = 




XQDS = 


,XQDB= 


,XRDR= 




,XW = 




XNN = 


,XVDR= 


,XWDS= 




,XWDB= 




XDSDS= 


,XDBDB= 


,XDRDR= 




,XQDSN= 




XNDSN= 


,XDSDSN= 










LATERAL HYDRODYNAMIC COEFFICIENTS 






CONST 


YPDOT= 


,YRDOT= 


,YPQ = 




,YQR = 




YVDOT= 


,YP = 


,YR = 




.YVQ = 




YNP = 


,YWR = 


,YV = 




,YVW = 




YDR = 


,CDY = 








X 


NORMAL HYDRODYNAMIC COEFFICIENTS 








CONST 


ZQDOT= 


,ZPP = 


,ZPR = 




,ZRR = 




ZNDOT= 


,ZQ = 


,ZVP = 




,ZVR = 




ZN = 


,zvv = 


,ZDS = 




,ZDB = 


* 


ZQN = 


,ZWN = 


,ZDSN= 




,CDZ = 




ROLL HYDRODYNAMIC COEFFITIENTS 








CONST 


KPDOT= 


, KRDOT= 


,KPQ = 




,KQR = 




KVDOT= 


, KP = 


,KR = 




,KVQ = 




KWP = 


, KWR = 


,KV = 




,KVW = 




KPN = 


, KDB = 










PITCH HYDRODYNAMIC COEFFICIENTS 








CONST 


MQDOT= 


, MPP = 


,MPR = 




,MRR = 




MHDOT= 


, MQ = 


,MVP = 




,MVR = 




MW = 


, MW = 


,MDS = 




,MDB = 




MQN = 


, MWN = 


,MDSN = 








YAW HYDRODYNAMIC COEFFICIENTS 








CONST 


NPDOT= 


, NRDOT= 


,NPQ = 




.NQR = 




NVDOT= 


, NP = 


,NR = 




,NVQ = 




NWP = 


, NWR = 


,NV = 




,NVW = 


¥ 


NDR = 










¥ 


MASS CHARACTERISTICS OF THE FLOODED MARK 


IX VEHICLE 


CONST 


WEIGHT = 


, BOY = 


WOL = 




,XG = 




YG = 


, ZG = 


,XB = 




,ZB = 




IX = 


, lY = 


,IZ = 




,IXZ = 




lYZ = 


, IXY = 


,YB = 








L = 


, RHO = 


^ G = 




,NU = 




AO = 


,KPROP = 


,NPROP = 




X1TEST= 




DEGRUD= 0.0 


,DEGSTN= 0.0 








CONST 


XOBS1=36.0 











CONST ZOBSl= -12.0 
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X INPUT INITIAL CONDITIONS HERE IF REQUIRED 

X 

INITIAL 

X 

X D5AVE1=5QRT((XP05M-X0B51)X(XP0SM-X0B51)+. . . 

X (ZP05M-Z0B51)X(ZP0SM-Z0BS1 )) 

X D3AVEV=DSAVE1 

X INITIALIZE ALL MATRICES AND ARRAYS TO ZERO 

NOSORT 

ORDDEP=20.0 
YORD=AO . 0 
D=0 
H = H + 1 

IF (H.EQ.l) THEN 
U ^ 6 



2 J = 1 


>N 




JJ= J+N 






DO 1 K 


= 1 


,N 


KK= K+N 






KKK= KK 


+ N 




MMINVC J, 


K) 


= 0 


X(J) = 0 


. 0 




X(JJ) = 


0 . 0 




XDOTC J ) 


= 0 


. 0 


XDOTC JJ) 


= 


0.0 


XDOTXC J ) 


= 


0 . 0 


XDOTXC JJ 


) = 


0 . 1 


XDOTUC J) 


= 


0 . 0 


UMODC J) 


= 0 


. 0 


MM( J,K) 


= 0 


.0 


BB(J,K) 


= 0 


. 0 


B(J,K) = 


0. 


0 


AA( J,K)= 


0 . 


0 


AA( JJ,KK 


) = 


0 . 0 


AA( J ,KK) 


= 0 


. 0 


AA( JJ,K) 


= 0 


. 0 


A( J,KK)= 


0 . 


0 


A( JJ,K) 


= 0 


.0 


A(J,K) = 


0 . 


0 


A( JJ,KK) 


= 0 


.0 


GKKC J,K) 


= 0 


.0 


GKK( J,KK 


)=0 


. 0 


GKK( J,KKK)= 


0 . 0 


CONTINUE 







2 CONTINUE 

X 

X INPUT THE LINEARIZATION POINT PARAMETERS 

X 

UO =6.0 
VO = 0.0 
WO = 0.0 
PO = 0.0 
QO = 0.0 
RO = 0.0 
PHIO = 0.0 
THETAO = 0.0 
PSIO = 0.0 
SUM = 0.0 
JFLAG = 0 
IFLAG = 0 
KFLAG = 0 
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^ INPUT THE MODEL STATES INITIAL CONDITIONS 

UM = 6 . 0 
VM = 0.0 
NM =0.0 
PM = 0 . 0 
QM = 0.0 
RM = 0.0 
XPOSM =0.0 
YPOSM =0.0 
ZPOSM =0.0 
PHIM = 0.0 
THETAM =0.0 
PSIM = 0.0 
U =6.0 
V =0.0 
W =0.0 
P =0.0 
Q =0.0 
R =0.0 
XPOS = 0.0 
YPOS =0.0 
ZPOS =0.0 
PHI =0.0 
THETA = 0.0 
PSI = 0.0 

INPUT THE VEHICLE INITIAL CONDITIONS 

^ INITIALIZE THE CONTROLS 

DELBOY= 0.0 
DBOY=0 . 

DS= 0.0 
DSM = 0.0 
DBM = 0.0 

DB=0.0 
DR= 0.0 
DRM=0.0 
DRPM=0 . 0 
RPM = 500.0 
LATYAN =0.0 
rJORPIT = 0.0 

X 

MASS = WEIGHT/G 

DIVAMP = DEGSTN^O . 017^552925 
RUDAMP = DEGRUD5(0 . 017A532925 

^ DEFINE LENGTH FRACTIONS FOR GAUSS QUADUTURE TERMS 

GA(1) = 0.069A318AA 
GA(2) = 0.330009A78 
GA(3) = 0.669990521 
GA(A) = 0.930568155 

DEFINE HEIGHT FRACTIONS FOR GAUSS QUADUTURE TERMS 

GKA(l) = 0 .173927A225687 
GKAC2) = 0 .326072577A312 
GKA(3) = 0 .326072577A312 
GKA(A) = 0 .173927A225687 
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>K 



>: 



DEFINE THE BREADTH BB AND HEIGHT HH TERMS FOR THE INTEGRATION 






X 

X 

X 

X 

X 



BR(1) = 75.7/12 
BR(2) = 75.7/12 
BR(5) = 75.7/12 
BR(A) = 55.08/12 

HH(1) = 16.58/12 
HH(2) = 51.85/12 
HH(5) = 51.85/12 
HH(A) = 25.76/12 

THE LINEAR PROPULSION MODEL 



ETA = 0.012^RPM/U0 
ETA = 1.0 
RE = UOXL/NU 

CDO = .00585 + (1.296E-17)X 

CT = 0.008^L^^2^ETA^ABS(ETA 
CTl = 0 . 008^LXX2/(A0) 

EPS = -1 .0+(SQRT(CT+l .0)-l .0 
XPROP = CDOX(ETA^ABSCETA) - 



(RE - 1 .2E7)^^2 
)/(A0) 

)/(SQRT(CTl+l .0)-l 
1 . 0 ) 



0 ) 



CALCULATE THE MASS MATRIX 



MM(1,1) = MASS -((RH0/2)X(LXX3)^XUD0T) 
MM(1,5) = MASSXZG 
MM(1,6) = -MASS^YG 



MM(2,2) = MASS - ( ( RHO/2 ) ^ ( L ^^5 ) ^YVDOT ) 
MM(2,A) = -MASS^ZG -( ( RHO/2 ) X( L)(^4 ) KYPDOT ) 
MM(2>6) = MASSXXG - ( ( RHO/2 ) x ( L^^4 ) )(YRDOT ) 



MM(5,5) = MASS - ( ( RHO/2 ) 5(( L^(X3) XZWDOT ) 
MM(3,4) = MASSXYG 

MM(5,5) = -MASS^XG -( ( RH0/2)X( L^^4)^ZQD0T) 



MM(4,2) 
MM(4,3) 
MM(4, 4) 
MM(4,5) 
MM(4,6) 



-MASS^ZG - ((RH0/2)X(L^^4)^KVD0T) 
MASSXYG 

IX -* ( (RHO/2)5((L)^5(5)^KPDOT) 

-IXY 

-IXZ ~((RH0/2)^(L^^5)^KRD0T) 



MM(5, 1) 
MM(5, 5) 
MM(5,4) 
MM(5,5) 
MM(5,6) 



MASS5(ZG 

-MASS^XG -( (RH0/2))((L^^4))(MWD0T) 
-IXY 

lY -((RH0/2)^(L^X5)XMQD0T) 

-lYZ 



X 

X 



20 

X 



X 



MM(6,1) = -MASSXYG 

MM(6,2) = MASSXXG - ( ( RHO/2 ) )( ( L^)(4 ) XNVDOT ) 
MM(6,4) = -IXZ - ( (RH0/2)^(L^X5)XNPD0T) 
MM(6,5) = -lYZ 

MM(6,6) = IZ - ( (RH0/2)^(L^(^5)^NRD0T) 



LAST = NXN+5XN 
DO 20 M = 1,LAST 
MKAREA(M) =0.0 
CONTINUE 

lER = 0 
lA = 6 
IDGT = 4 
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CALL ‘■I'^V2F(MM,»,IA,MMINV,IDGT,WKAREA,IER) 

CALCULATE THE A MATRIX FOR THE LINEAR MODEL 



A(l,l) = 



A(1,2) 

A(1,3) 

A(1,A) 

A(1,5) 

A(1,6) : 

AC1,11): 

A(2,1) = 

A(2,2) = 
A(2,3) = 
A(2,A) = 

A(2,5) = 

A(2,6) = 

A(2,10)= 

A(2,ll)= 

A(3,1) = 



A(3,2) 

A(3,3) 

A(3,A) 



A(3,5) 

A(3,6) 



RH0/2^axx2x(XVDRxvSR+XWn^^n^°n'"^'^°'^^'^°'^^'^0^DR) + 
2 xU0X(xdSDSkds!(X 2 + XDBDB/?^nRptvt^'‘^°®^23^DBPKM0 + ^ 
RH0/2ML3(K3XXQDSN^00XDSXFP^+D2nyof? >^DRDR5(DRXX2)) + 

2^^pSDSN5fUOXDS5fj(2)XEPS+RHO?(^lf¥7¥nn!!vD«^^‘‘^^^^^^'‘^03(DS+. ' ' 

: XVR.RO, * RH0.2.UX2. ..,' 

- “MASSKYGJ^QO-MASSKZn^pnl PLjn^^ +XWDSNXU03(DSXEP3 i 

) + RH0/2>(Lxjf2?((YVxV0+. 

MASSxPO+^RHO/2XL°x3x?YWP¥Pn^v^^'^^'^°''’^'' 

MASSXWO-MASS5(XGJ(QO + 2XMA<;<;CYrvPn^D® 
RHO/2KLKK3K(YPKUOr?K?f°^PO+RHO/2^L^^AKYPQ^ 

“"“^|»|'°»3»"?R«JJ°I'*?,i^5“*^<5*0»+RH0/2«L»«A»V«R»l)0 +. . . 
p«UO?2DB/KDBP«SHoJ2K5n^l»i'lJ*^*'^“*““*2 

RHO/2«t«,3,ZQ,%JJ*«g+2»"XSS»ZG«00+RH022,|.,j,5*2o,„„ ^ 
■«-SS«XG»P0-«ASS»m,0*RHO22XL«»A«(2pp,p„,,,,,p,p„,^ ■ • 
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FILE: TX 



DSL 



A1 



A(3,10)= 

A(3,ll)= 

A(A,1) = 

A(A,2) = 
A(A,3) = 
A(A, A) = 
A( A , 5) - 
A(A,6) = 
A(A, 10)= 
ALA, 11) 

A(5,l) 

A(5,2) 

A(5,3) 
ACS, A) 

ACS, 5) 

ACS, 6) 

ACS, 10) 
ACS, 11) 

AC6,1) 

AC6 ,2) 

AC6,3) 

AC6, A) 

AC6,5) 

AC6,6) 

AC6,10 

AC6,11 



- a°E?GHf -io? ) M?0S C THET AO ) I N C PHI 0 ) 

-CWEIGHT - BOY)?«SIHCTHETAO)MCOSCPHIO) 

MASS^YG5(Q0 + f^^^|^?°w^wn + ?xU0XCKDB/2^DBP-KD^ 

-MaSgxPO + RH0/2ML^5(AMCKWPMP0 + KWRHRO) + ... 

RH0/2xL?f?<3J^KVW^y0 _ maSSJ«ZG5(H0 + ... 

il;ii:rii:3lps=.- 

liisliiililr' 

-(YG3<11EIGHT-YB?(BOY)^SINCTHETAO)^COSCPHIO) . . . 

+CZGKWEIGHT-ZB^BOY)XSIMCTHETAO)XSINCPHIO 

i -HASS«XO«00 + 5SK?ni!lMM/SSBP) ^"rhO/zKxxaSoNXQO* . 

= , RH0/2«L«««(MVP,P0 .... 

MVRJ^RO) + + RH0/2XLXX55(MHN5(U0^EPS 

= -MAS55(ZGKQ0 + RHO/2KLKM5^MW^yO +pKMU ^^33^xG5(V0 + ... 

L^kA^MQ 5(U0 + R^^°''fYY¥on^ + '^2«XZ^R0 + MASS^^ZG^^VO +... 

1= (XGS<WEIGHT-XB^BOY)^SIMCTHETAO)XCOSCPHIO) 

CZG^HEIGHT-ZB5(B0Y)^C0SCTHETA0) 

= SSgxPO , mSSXYGXQO * RHO/2.l»««NWP»P0*N“«’<''“>* 

RHO/2«L»«5»mW*VO +IYZXRO ♦ «ASS«XC!iHO+ . . • 

= »f«»oo,*,«^r,r^*fx2^Rr“,"S . . . 

■ Rio^mxfsxMORXQO 



B2 



>4< >K >K 



A(7,l) = COSCPSIO )XCOS(THETAO) 

A(7,2) = COS(PSIO)?(SItKTHETAO)XSIN(PHIO) - SItK PSI 0 )?(COS( PHIO ) 
A(7,3) = COS(PSIO)XSIN(THETAO)KCOS(PHIO) + SI N ( PSI 0 ) JfSIN ( PHI 0 ) 
A(7,10)= V03^C0S(PSI0)3(SIN(THETA0)XC0S(PHI0) + V O^SIfH PSI 0 ) ^ . . . 

SIN(PHIO) - WO?«COS(PSIO)XSIIKTHETAO)KSIN(PHIO) + ... 
HOKSIN(PSIO)5«COS(PHIO) 

A(7,1I)= -UO>ECOS(PSIO)5<SIN(THETAO) + V05«COS ( PS 1 0 ) ?«COS( THET AO ) ^ 
SIN(PHIO) + WO?«COS(PSIO)XCOS(THETAO)?fCOS(PHIO) 
A(7,12)= -UO^SIN(PSIO)?fCOS(THETAO) - VOKSI N ( PSI 0 ) ?<SI N ( THETAO ) X 
SIN(PHIO) - VO?«COS(PSIO)XCOS(PHIO) - HO^SI M ( PSI 0 ) ?« . . . 
SltKTHETAO)XSIN(PHIO) + HO?<COS ( PSI 0 ) ?(SI N( PHI 0 ) 

A(8,l) = SIIKPSIO))(COS(THETAO) 

A(8,2) = SIN(PSIO)XSIN(THETAO)?«SIN(PHIO) + COS ( PSI 0 ) ^COS ( PHI 0 ) 
A(8,3) = SIM(PSIO)^SIN(THETAO)5(COS(PHIO) - COS( PSI 0 ) KSIN( PHIO ) 
A(8,10)= V05CSIN(PSIO)XSIN(THETAO)XCOS(PHIO) - VOKCOSC PSIO ) ^ . . . 

SIN(PHIO) - WOXSIN(PSIO)?«SIN(THETAO)KSIN(PHIO) - ... 

HOXCOS(PSIO)XCOS(PHIO) 

A(8,ll)= -U05«SIIKPSI0)?«SIfKTHETA0) + VO?«SI (U PSI 0 ) ?«COS ( THETAO ) 
SIN(PHIO) + WOXSIN(PSIO)XCOSCTHETAO)^COS(PHIO) 

ACS, 12)= UO^C05(PSIO)MCOS(THETAO) + VOKCOS(PSIO)?(SIN(THETAO)?« . 

SINCPHIO) - VOXSIfUPSIO)?(COS(PHIO) + WOXCOS ( PSI 0 ) . . . 
SIN(THETAO)KCOSCPHIO) + WOXSI N ( PSI 0 ) KSI N ( PHI 0 ) 

A(9,l) = -SIIKTHETAO) 

A(9,2) = COSCTHETAO)?(SIN(PHIO) 

A(9,3) = COSCTHETAO)XCOS(PHIO) 

A (9, 10)= VOXCOS ( THETA 0)5«COS( PHI 0)-W0XCOS(THETA0)?«SIN( PHI 0) 
A(9,ll)= -UOXCOS(THETAO)-VO^SIN(THETAO)XSIN(PHIO) -... 
WOXSIN(THETAO)XCOSCPHIO) 



A(10,A) = 1.0 

A(10,5) = SltUPHIO)JfTAN(THETAO) 
A(10,6) = COS(PHIO)KTANCTHETAO) 
A(10,10)= QOXCOS(PHIO)XTANCTHETAO) - 
AC 10, 11 )= QOXSIIKPHIO)/COSCTHETAO)^1 , 
ROXCOSCPHIO)/COSCTHETAO)X1 , 



R0XSINCPHI0)XTANCTHETA0) 
O/COSCTHETAO) + . . . 
O/COSCTHETAO) 



AC11,5) = COSCPHIO) 
AC11,6) = -SINCPHIO) 
AC11,10)= -QO?(SINCPHIO) - 



RO?<COSCPHIO) 



AC12,5) = SINCPHIO)/COSCTHETAO) 

AC12,6) = COSCPHIO)/COSCTHETAO) 

AC 12, 10)= QOXCOSCPHIO)/COSCTHETAO)-R03<SINCPHIO)/COSCTHETAO) 
A(12,ll)= QO^SINCPHIO)/COSCTHETAO)XTANCTHETAO) + ... 
RO?fCOSCPHIO)/COSCTHETAO)xTANCTHETAO) 



WRITEC10,200)CCACI, J), J=1,12),I=1,12) 
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K 



X 

X 

X 

X 



CALCULATE THE B MATRIX 



B(l,l) = 

B(l,2) = 
B(l,3) = 
B(l,^) = 



B(l,5) 

B(l,6) 



RHO/2XL^x3XXRDR^UO^RO + RHO/2)^L^^2)^(XRDR^UOX\/0 + UOXX2X . . 
2 ^XDRDR^ DR ) 

UOXQOXXQDB/2 + U0XW0)(XWDB/2 + U0XX2)cXDBDB^DBS 
UO^QO^XQDB/2 + UO^NO^XWDB/2 + U0XX2XXDBDB^DBP 
UOXQOXXQDS + UOXWOXXNDS +UO^X2)^2XXDSDS^D5 + RHO/2XLXX3X 
XQDSNXUO^QOXEPS + RH0/2^LXX2)^ ( XNDSN^UOXWO + 2^XDSDSN^ 
U 0 )^X 2 )^D 5 )XEPS 

RHO/2^L^^2^0 . 12^CDO^O . 12^RPM 
SIN(THETAO) 



B(2,l) = RHO/2^LXX2^YDR^U0^^2 
B(2,6) = '-COS(THETAO)^SirKPHIO) 



B(3,2) = 
BC3,3) = 
B(3,A) = 
BC3,6) = 



U 0 ^^ 2 ^ZDB/ 2 ^RHO/ 2 ^L^X 2 
UO^X 2 ^ZDB/ 2 ^RHO/ 2 ^L ^^2 
UOXX2XZD5XRHO/2XLXX2 + 
-COS(THETAO)XCOS(PHIO) 



RHO/ 2 ^L^^ 2 XZDSNxUOXX 2 XEPS 



B(^,2) =-RHO/2^L^X3xU0XX2MKDB/2 
B(A,3) = RHO/2XL^x3xUOXX2xKDB/2 
B(A,6) = -YBXCOSCTHETAO )XCOS(PHIO) 



+ ZBXCOS(THETAO)K3IN(PHIO) 



B(5,2) = RHO/2^Lxx3^UOX^2XMDB/2 
B(5,3) = RHO/2^L^^3 ^UOxx2xMDB/2 
B(5, A ) = RH0/2XL^X3^( U0xx2xMDS+MDSN^U0^^2^EPS) 

B(5,6) = XBXCOS(THETAO)^COSCPHIO) + ZBXSI N ( THETAO ) 

B(6,l) = RHO/2XL^)(3 xNDR)(U0xx2 

B(6,6) = -XBXCOS(THETAO))^SIN(PHIO) - YB^SINC THETAO ) 
FORMULATE THE A AND B MATRIX FOR STATE SPACE REPRESENTATION 
MULTIPLY MMINV AND DF/DX 



DO 80 I = 1,6 
DO 70 J = 1,6 



BA 



I 



FILE: 


TX DSL 


A1 




SUM =0.0 






DO 60 K = 


1,6 




SUM = SUM 


+ MMINV(I,K)^A(K, J) 


60 


CONTINUE 






AA(I,J) = 


SUM 


70 


CONTINUE 




80 


CONTINUE 








^ MULTIPLY MMinV AND DF/DZ 

DO 50 I = 1,6 

DO ^0 J = 7,12 
SUM =0.0 

DO 50 K = 1,6 

SUM = SUM + MMINV(I,K)^A(K, J) 

50 CONTINUE 

AA(I,J) = SUM 
AO CONTINUE 

50 CONTIf^UE 

DO 5 I = 7,12 
DO 6 J = 1,12 
AA(I,J) = A(I,J) 

6 CONTINUE 

5 CONTINUE 

X 

NRITE(10,200)((AA(I, J), J=1,12),I=1,12) 
200 FORMATC 6E12.A) 

^ MULTIPLY MMINV AND DF/DU 

K 

DO 110 I = 1,6 
DO 100 J = 1,6 
SUM =0.0 
DO 90 K = 1,6 

SUM = SUM + MMINV(I,K)^B(K, J) 

90 CONTINUE 

BB(I,J) = SUM 
100 CONTINUE 

no CONTINUE 

HRITEC 9,500)((BB(I,J),J=1,6),I=1,6) 
500 F0RMATC6E12. A) 

X 

X 

DO A05 I = 1,6 

READ (2,A01)(GKK(I, J), J=l,21) 

A 05 HRITECS, AODCGKKCI, J), J = l,21) 

AOl FORMATC5E20.10) 

X 



B5 



ELSE 
EUD IF 

CALL DADS(INFO,ISTRAT,IOPT,IONED,IPRINT,IGRAD,NDV,NCON,DX, . . . 

VLB, VUB, OBJ,GW, IDG, NGT, IC, DF, AW,NRA, NCOLA, WK, NRNK, . . . 
IWK,NRINK) 

IF (INFO.EQ. 0) D£LPRT=0.2 

DERIVATIVE 

NOSORT 

LATYAN =0.0 
NORPIT =0.0 

INEAR 

X 

^ CALCULATE BB^U PART OF XDOT = AA^^X + BB^^U 
)( 

DO 10 J = 1,6 
SUM =0.0 
DO 15 K = 1,6 

SUM = SUM + BBC J,K)^(UMOD(K) 

15 CONTINUE 

XDOTUCJ) = SUM 
10 CONTINUE 
^ CALCULATE AA^^X 

DO 21 J= 1,12 
SUM =0.0 
DO 25 K = 1,12 

SUM = SUM + AAC J,K)^(X(K) 

25 CONTINUE 

XDOTXCJ) = SUM 
21 CONTINUE 

^ CALCULATE XDOT = AA^X + BB^U 
DO 31 J = 1,6 

XDOTCJ) = XDOTXCJ) + XDOTUCJ) 

31 CONTINUE 

DO 35 J = 7,12 

XDOTCJ) = XDOTXCJ) 

35 CONTINUE 

UDOTM = XDOTCl) 

VDOTM = XD0TC2) 

MDOTM = XD0TC3) 

PDOTM = XDOTCA) 

QDOTM = XD0TC5) 

RDOTM = XD0TC6) 

XDOTM = XD0TC7) 

YDOTM = XD0TC8) 

ZDOTM = XD0TC9) 

PHMDOT= XDOTCIO) 

THETMD= XDOTCll) 

PSMDOT= XD0TC12) 

HRITEC8,600) 

^ INTEGRATE XDOT TO GET THE STATE VECTOR X 



6 5 



>K >K >K 



UM =INTGRL(6.0, UDOTM) 

VM= INTGRLCO.O, VDOTM) 

HM= IMTGRLCO.O, NDOTM) 

PH= ir^TGRLCO.O, PDOTM) 

OM= ir^TGRLCO.O, QDOTM) 

RM= INTGRLCO.O, RDOTM) 

XPOSM = iriTGRLCO.O, XDOTM) 

YPOSM = INTGRLCO.O, YDOTM) 

ZPOSM = INTGRLCO.O, ZDOTM) 

PHIM = INTGRLCO.O, PHMDOT) 

THETAM = INTGRLCO.O, THETMD) 

INTGRD = (UM^UM+VM^VM+WM^NM+PM^PM+QM^QM+RM^RM+. . . 

XPOSM^XPOSM+(YPOSM-YORD)K(YPOSM-YORD)+. . . 
(ZPOSM-ORDDEP)x(ZPOSM“ORDDEP)+ PHIM^PHIM+. . . 
THETAMXTHETAM+PSIM^PSIM) + ( DSM^DSM+DBSM^DBSM ) + . . 
(DBPM^DBPM)+(DRM^DRM) 

OBJl = INTGRL(0.,(0.5)^iraGRD) 

OBJ = OBJl 

PSIM = INTGRLCO.O, PSMDOT) 



XCl) 


= 


UM 


XC2) 


= 


VM 


XC3) 


= 


NM 


XC A) 


= 


PM 


XC5) 




QM 


XC6) 


= 


RM 


XC7) 


r 


XPOSM 


XC8) 


= 


YPOSM 


XC9) 


= 


ZPOSM 


XC 10) 




= PHIM 


XCll) 




= THETAM 


XC12) 




= PSIM 






ZDEPTH 

THMANG 

UMODCl) 

UM0DC2) 

UM0DC5) 

UMODC A) 

UM0D(5) 

UMODC6) 



ZORD - XC9) 
XC11)^57 .3 
DRM 

= DBSM 
= DBPM 
= DSM 
= DRPM 
= DBOY 



PHANG=PHIM/0 .017^532925 
THMANG=THETAM/0 . 017A532925 
PSMAfJG= PSIM/ 0.017A532925 
PITCHM=THMANG 
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^:xx^^xCO[iTROL L 



X 



X 

X 



X 

X 

X 

X 

X 

X 

X 

X 

X 



X 

X 

X 

X 

X 

X 



DBS 

DBF 

DS 

DR 

DBS 



DBF 



DS 



UM0DC2) 

UM0DC3) 

UMOD(^) 

UMOD(l) 

-(GKK(2,1)^U + GKK(2,2)^V + GKK(2,3)^N + GKK(2,‘4)xp +... 
GKK(2,5)XQ + GKK(2,6)^R + GKK( 2 , 7 ) XXPOS + GKK( 2 , 8 ))(YPOS +... 
GKKC2, 9)^ZPOS + GKK ( 2 , 1 0 ) ^PHI + GKK( 2 , 1 1) ^THETA + ... 

GKKC2, 12)^PSI + GKK(2,13)xm + GKKC 2 , 1 ) ^QM + GKK( 2 , 1 5 ) ^ . . . 
ZPOSM + GKK(2,16)XTHETAM + GKK( 2, 17 ) ^UMODC 2) + GKK( 2, 18 ) X . . . 
UM0DC3) + GKK(2,19))(UM0D(A)) 

-(GKK(3,1)^'U + GKK(3,2)X'V + GKK(3,3)^W + GKK( 3 ,A)xp +... 
GKK(3,5)XQ + GKK(3,6)XR + GKK ( 3 , 7 ) XXPOS + GKK( 3 , 8 ) XYPOS +... 
GKK(3,9)XZP0S + GKK ( 3 , 1 0 ) xpHI + GKK( 3, 1 D^THETA + ... 

GKK(3, 12)XPSI + GKK(3,13)^WM + GKK(3,1A)XQM + GKK(3,15)X_. 
ZPOSM + GKK(3,16)^THETAM + GKK( 3, 17 )XUM0D( 2) + GKK( 3 , 18 )5( . . . 
UM0DC3) + GKK(3,19)XUMOD(A)) 

-(GKK(A,1)XU + GKK(A,2)XV + GKK(A,3)^M + GKK(A,A)Xp +... 
GKK(^4,5)^Q + GKK(^,6)XR + GKK ( A , 7 ) 3(XPOS + GKK ( A , 8 ) ^YPOS +... 
GKK(9,9)XZP0S + GKK ( A , 1 0 ) ^PHI + GKK ( A , 1 1) ^THETA + ... 

GKKsC A, 12)XPSI + GKKC^, 13)^MM + GKK ( , 1 ^ ) ^QM + GKK( ^ , 1 5 ) ^ . . . 
ZPOSM + GKK( A, 16)^THETAM + GKK ( A , 1 7 ) ^UMODC 2 ) + GKK( A , 18 . . . 
UM0DC3) + GKK(A,19)XUM0D(A)) 



^ PUT IN STERN AND BON PLANE STOPS 

X 

X IF(ABS(DBS) .GT .0.6) THEN 

X DBS = 0.6XABS(DBS)/DBS 

X ENDIF 

X IFCABS(DBP) .GT.0.6) THEN 

X DBP = 0 .6xaBS(DBP)/DBP 

X ENDIF 

X IF(ABSCDS) .GT.0,6) THEN 

X DS = 0 .6xaBS(DS)/DS 

X ENDIF 

X 



xxxxxxnON-LINEAR 

X 

X PROPULSION 

X 



MODELXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX 

MODEL 



X 



SIGNU =1.0 
IF (U.LT.0.0) SIGNU = 
IF (ABS(U) .LT.XITEST) 
SIGNN =1.0 
IF (RPM.LT.0.0) SIGNN 
ETA = 0.012XRPM/U 



-1 .0 

U = XITEST 



= -’ 1.0 



8c 



>K >K >K 



I 



FILE: TX 



DSL A1 



X 

X 

X 

X 

X 

X 

X 



500 

X 

X 

X 

X 

X 

X 

X 



RE = U^l/UU 

CDO = .00385 + ( 1 .296E-17)^(RE - 1.2E7)XX2 

CT = 0 . 0085(L)^^2)^ETA)(ABS(ETA)/(A0) 

CTl = 0 . 0085(LXX2/(A0) 

EPS = -1 ,0 + SIGNM/SIGNU5^(SQRT(CT + 1.0)“l .0)/(SQRT(CTl + l .0)-l .0) 
XPROP = CDOX(ETA^ABSCETA) - 1.0) 



CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER THE VEHICLE 
INTEGRATE USING A A TERM GAUSS QUADUTURE 



LATYAH =0.0 
NORPIT = 0.0 
DO 500 K = 1,9 

UCF(K) = SQRT( (V + G9(K)5^RXL)^5(2 + ( W-G9 ( K) XQXL ) xx2 ) 
IF(UCFCK) .GT. lE-10) THEN 

TERMO = (RH0/2)X(CDYXHH(K)^(V + G9(K)5^RXL))^)(2 +... 

CDZ^BR(K)^(N-G9(K))^Q^.L))(X2) 

TERMl = TERM0 5^(V+G9(K)5(RXL)/UCF(K) 

TERM2 = TERM0X(W-G9(K))^Q5^L )/UCF(K) 

LATYAW = LATYAH + TERMl 5(GK9 ( K ) ^L 
NORPIT = NORPIT + TERM25(GK9 ( K ) ^L 
END IF 
CONTINUE 



FORCE EQUATIONS 



LONGITUDINAL FORCE 

FP(l) = MASS)(V^R - MASS^^WXQ + MASS)(XG5^Q5(x2 + MASS5(XG5^R)(5(2- . . . 

MASSKYG^PXQ - MASSXZO^PXR + ( RHO/ 2 ) XL ( XPPXPXX2 +... 
XQQXQXX2 + XRRXRXX2 + XPRxpXR) +( RHO/2 )XLXX3X( XHQ^N)(Q +. 
XVPKV5(P+XVR)(VXR+U)^Q5^(XQDS)(DS+XQDB/2)^DBP) + XRDRXUXRXDR)+ . . 
(RH0/2)xLxx2)((XVV^Vxx2 + XNN)(pmx2 + XVDRxU^V^DR + U^W)^.. 
(XHDSXDS+XWDB/2XDBP) + UXX2>^'(XDSDS5(DSX^2+XDBDB/25(DBP5(X2+ . . 
XDRDRXDR^X2) )-(HEIGHT -BOY)X$IN(THETA ) + ( RHO/2 ) ^ .. 

XQDSNXUXQXDSXEPS+(RH0/2)>^LK5^2X(XNDSNXU)(WXDS+XDSDSNXU)()^2)^ 
DS^^2)^EPS +CRHO/2)5(L5(X2^U5(X2)(XPROP + RHO/2^L5(X55(U)^Q^ . . . 
XQDB/25(DBS +RHO/2^U^)^2)(U5(X2^XDBDB/2)^DBS5^)^2+ . . . 
RH0/2xlxx2xXNDB/2^DBSxU)(W 



LATERAL FORCE 



FP(2) = -MASSXUXR + MASS^^XGXPXQ + MASSXYG)(R)^5^2 - MASS^ZO^Q^^R +. 

(RH0/2)XL)()(9X(YPQXpXQ + YQR)(QXR) + ( RH0/2)XLXX3)(( YP)(U^P +. 
YR^[}^R + YVQXV^^Q + YNP)(W)(P + YWR^WxR) + ( RHO/2 ) 5(L ^^2^ . 

(YV5(U5(V + +ydr)^U5(x25(dr) -latyaw +(height-boy))(. . 

COS(THETA)XSIN(PHI) 
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NORMAL FORCE 

FP(5) = MASS^U^Q - MASS^V^P - MASS^XG^P^R - MASS^YG^Q^^R + 

MASS>'ZG^P^X2 + MASS^ZG^Q^xZ + ( RHO/2 ZPP^P^^Z +. 

ZPR^P^R + ZRR5^R^^^2) + ( RHO/2 ) ( ZQ^^U^Q + ZVP^V^P +. 

ZVRXV^R) +(RH0/2)XL^^2^(ZH^U^H + ZVVXVxxZ + U^^2^(ZDS^. 
DS+ZDB/2^DBP) )-rJORPIT+(NEIGHT-BOY)5^COS(THETA)^COS(PHI ) + 
CRH0/2)XLXX3^ZQN^U^QXEPS + ( RHO/2 ) XL XX2X ( ZNNXUXW +ZDSNX. 
UXX2XDS)XEPS+ RH0/2XLXX2XUXX2XZDB/2XDBS 



ROLL FORCE 

FP(A) = -IZXQXR +IYXQXR -IXYXPXR +IYZXQXX2 -IYZXRXX2 +IXZXPXQ + 
MASSxyGXUxQ -MASSXYGxvxp -MAS$xZGXWxp+( rhO/ 2 )XLXX5X( KPQX 
PXQ + KQRXQXR) +(RH0/2)XLXXAX(KPXUXp +KRXUXR + KVQXVXQ + 
KWPXWXP + KHRXWXR) +( RHO/2 )XL XX3X( KVXUxv + KVWXVXM) + . 

(YGXNEIGHT - YBXB0Y)XC0S(THETA)XC0S( PHI) - (ZGxNEIGHT - 
ZBxbOY)XCOS(THETA)xSIN(PHI) + (RH0/2)XLXXAXKPNXUXPXEPS + 
(RH0/2)XLXX3XUXX2XKPR0P +MAS$XZGXUXR+ ... 

RH0/2XLXX3XUXX2XCKDB/2XDBP-KDB/2XDBS) 



X 

X 

X 



X 

X 

X 

X 

X 



X 

X 

X 



X 

X 

X 

X 

X 



X 



PITCH FORCE 

FP(5) = -IXxpxR +IZXPXR +IXYXQXR -lYZxpxQ -IXZxpxx2 +IXZXRXX2 

MASSXXGXUXQ + MASSXXGXVXP + MASSXZGXVXR - MASSXZGXN^^'Q +... 
(RH0/2)XLXX5X(MPPXPXX2 +MPRxpxR +MRRXRxx 2 )+( RHO/2 )XLXXAX . . 
(MQXUXQ + MVPXVXP + MVRXVXR) + ( RHO/2 ) XLXX3X( MHxUxw + ... 

MVVXVXX2+UXX2XCMDSXDS+MDB/2XDBP) )+ NORPIT -(XGXHEIGHT- ... 
XBxB0Y)XC0S(THETA)xC0S(PHI) +. . . 

( RH0/2)XLXX3X(MNNXUXW+MDSNXUXX2XDS)XEPS+ RH0/2XLXX3X. . . 
UXX2XMDB/2XDBS-(ZGXNEIGHT-ZBXB0Y)XSIN( THETA) 

YAN FORCE 

FP(6) = -lYxpxQ +IXXPXQ +IXYXPXX2 -IXYXQxx2 +IYZxpxR -IXZXQXR 

MASSXXGXUXR + MASSXXGXWXp - MASSXYGXVXR + MASSXYGXNXQ +... 
(RH0/2)XLXX5X(NPQXPXQ + NQRXQXR) + ( RHO/2 )XL xxAx( NPxUxp+ . . . 
NRXUXR + NVQXVXQ +NHPxWxp + NWRXWxR) + ( RHO/2 ) XL xx3x ( NVx . . . 
UXV + NVWXVXN + NDRXUXX2XDR) - LATYAN + (XGXWEIGHT - ... 

XBXB0Y)XC0S(THETA)XSIN(PHI)+(YGXWEIGHT)XSIN(THETA) . . . 

+ ( RHO/2) XL xx3XUXX2xNPR0P-YBXB0YXSirU THETA) 

IF(Z.EQ.50.0)THEN 

WRITE (S,500)(FP(I), I = 1,6) 

Z = 0.0 
ErJD IF 



NOW 


COMPUTE 


TH 


E FCl-6) 


FUNCTIONS 


DO 


600 


J = 


1, 


6 








F( J 


) = 


0.0 




DO 


600 


K = 


1, 


6 








F( J 


) = 


MMINVC J. 


,K)XFP(K) 



CONTINUE 



THE LAST SIX EQUATIONS COME FROM THE KINEMATIC RELATIONS 

FIRST SET THE DRIFT CURRENT VALUES 

UCO =0.0 
VCO =0.0 
NCO =0.0 
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INERTIAL POSITION RATES FC7-9) 






X 

5( 

¥. 

5(00 



3( 

X 

X 

X 



F(7) = UCO + U5(COS(PSI)3(COS(THETA) + 
SIN(PHI) - SIN(PSI)5(C0S(PHI) ) 
COS(PHI) + SIN(PSI)5(SIN(PHI)) 



V5((COS(PSI)5(SIN(THETA)3(. . 
+ N3((C0S(PSI )5(SIN(THETA)5( 



F(8) = VCO + U3(SIN(PSI)5(COS(THETA) + Vx ( SI N( PSD 5(SI N( THETA ) K . . 

SIN(PHI) + COS(PSI)5(COS(PHI) ) + NX ( SIN ( PSD XSI N ( THETA ) X 
COS(PHI) - COSCPSDXSINCPHD ) 

F(9) = NCO - UXSIN(THETA) +VXCOS(THETA)xSIN( PHD +NXCOSCTHETA) 
COS(PHI) 

EULER ANGLE RATES F(10-12) 

F(I0) = P + QXSIlKPHDXTAtKTHETA) + RXC0S(PHI)XTAN(THETA) 

F(ID = QXCOS(PHI) - RXSIN(PHI) 

F(12) = QXSINCPHD/COSCTHETA) + RxCOS ( PHD/COS ( THETA ) 

IF (Z.EQ.I .0)NRITE (9,500)(F(D, I = 1,12) 



F0RMATC6E12 


.9) 


Z = z + 


1 




UDOT = 


F(l) 




VDOT = 


F(2) 




NDOT = 


F(3) 




PDOT = 


F(<4) 




QDOT = 


F(5) 




ROOT = 


F(6) 




XDOTA= 


F(7) 




YDOT = 


F(8) 




ZDOT = 


F(9) 




PHIDOT 


= F( 


10) 


THETAD 


= F( 


ID 


PSIDOT 


= F( 


12) 



U = INTGRL(6.0,UDOT) 

V = ItJTGRL(0.0,VDOT) 

N = INTGRL(0.0,HDOD 
P = INTGRL(0.0,PDOT) 

Q = INTGRL(0.0,QDOT) 

R = INTGRL(0.0,RD0T) 

XPOS = INTGRL(0.0,XDOTA) 
YPOS = INTGRL(0.0,YDOT) 

ZPOS = INTGRL(0.0,ZD0T) 

PHI = INTGRL(0.0,PHIDOT) 
THETA = INTGRLC 0 . O/THETAD) 
PSI = INTGRL(0.0,PSIDOT) 

ZNEN = -ZPOS 

PHIANG = PHI/0 . 0I7A532925 
THEArJG = THETA/0 . 0I7A532925 
PSIANG = PSI/0 . 0179532925 
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DYNAMIC 

RN = TIME/(FINTIM/10 .-DEL T/1 0 0 00 . ) 

PN=TIME/( PINT IM/20 .-DELT/ 10000 . ) 

0=INT(RN)+1 
PP=INT(PN)+1 
IF(PP.GE.20) PP=20 
IF(O.EQ.ll) 0=10 

X ADDITIONALLY THE PLANES SHOULD BE AT EQUILIBRIUM SO THE 
X VEHICLE WILL PROCEED AT THIS NEW DEPTH WITHIN SOME TOLERANCE 

X 

DSM=DX(0) 

DBSM=DX(10+0) 

DBPM=DX(10+0) 

IF(O.GE.IO) DSM=0. 

IF(O.GE.IO) DBPM =0.000 
IF(O.GE.IO) DBSM =0.000 
DRM=DX(20+PP) 

X RPM=DX(30+0) 

X 

CONSTRAINTS FOR A DIVE 

^ ORDERED DEPTH = ORDDEP 

GW(1) = (ZP0SM-0RDDEP)?^.5 
GW(2) = (ORDDEP-ZPOSM))^ .5 

X AUV»S FINAL STATE MUST BE LEVEL FLIGHT AS FOLLOWS 
GW(3) = THETAM^elO. 

GWC^^) = -THETAMXIO. 

GW(5)=(YP0SM-Y0RD)/ 

GW(6)=(Y0RD-YP0SM)/.4 
GWC7) = -ZPOSM 

AVOIDING THE OBSTACLE 

IF (DIST1.lt. DSAVE l) DSAVE1=DIST1 



X IF (DISTV.lt. DSAVE l) DSAVEV=DISTV 

^ DIST1=SQRT( (XP0SM-X0BS1)X(XP0SM-X0BS1)+. . . 

(ZPOSM-ZOBSl)X(ZPOSM-ZOBSl) ) 

X DISTV=SQRT( (XP0S-X0BS1)X(XP0S-X0BS1)+. . . 

X (ZPOS-ZOBSl)^(ZPOS-ZOBSD) 

^ GW(8) = (1. -DSAVEl) 

NDX=XP0SM/17 . A25 
NDZ = ZP0SM/17 
NDT = TIME>(6./17.^25 
TERMINAL 

IF(INFO.EQ.O) THEN 
PRINTS, DSAVEl, DSAVEV 
9000 CONTINUE 
ELSE 
ENDIF 

IF(INFO.EQ.O) CALL ENDJOB 
CALL RERUN 
X 

END 

STOP 
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